From 6d4d42ecd7a7652fa83f9aaf87c9f77905a6c787 Mon Sep 17 00:00:00 2001 From: ohowe Date: Thu, 9 Nov 2023 21:51:35 -0700 Subject: [PATCH 01/16] Robot side line done; glass side major WIP --- glass/src/lib/native/cpp/other/Canvas2D.cpp | 29 +++++++ .../lib/native/include/glass/other/Canvas2D.h | 45 +++++++++++ glass/src/libnt/native/cpp/NTCanvas2D.cpp | 24 ++++++ .../include/glass/networktables/NTCanvas2D.h | 38 +++++++++ myRobot/src/main/java/frc/robot/MyRobot.java | 17 +++- .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 77 ++++++++++++++++++ .../wpilibj/glass/CanvasComplexObject2d.java | 5 ++ .../wpi/first/wpilibj/glass/CanvasLine2d.java | 78 +++++++++++++++++++ 8 files changed, 311 insertions(+), 2 deletions(-) create mode 100644 glass/src/lib/native/cpp/other/Canvas2D.cpp create mode 100644 glass/src/lib/native/include/glass/other/Canvas2D.h create mode 100644 glass/src/libnt/native/cpp/NTCanvas2D.cpp create mode 100644 glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp new file mode 100644 index 00000000000..b0ec52f2dc3 --- /dev/null +++ b/glass/src/lib/native/cpp/other/Canvas2D.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 "glass/other/Canvas2D.h" + +#define IMGUI_DEFINE_MATH_OPERATORS +#include +#include + +using namespace glass; +namespace gui = wpi::gui; + +void glass::DisplayCanvas2D(Canvas2DModel *model, const ImVec2 &contentSize) { + if (contentSize.x <= 0 || contentSize.y <= 0) { + return; + } + + ImGui::InvisibleButton("background", contentSize); + auto drawList = ImGui::GetWindowDrawList(); + + auto lines = model->GetLines(); + + // TODO: make this traverse multuple lists + for (auto line : lines) { + drawList->AddLine(line->GetPoint1(), line->GetPoint2(), line->GetColor(), + line->GetWeight()); + } +} diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h new file mode 100644 index 00000000000..69ef6a99804 --- /dev/null +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -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. + +#pragma once + +#include + +#define IMGUI_DEFINE_MATH_OPERATORS +#include + +#include "glass/Model.h" +#include "glass/View.h" + +namespace glass { + +class Canvas2DLineModel : public Model { + public: + virtual ImVec2 GetPoint1() const = 0; + virtual ImVec2 GetPoint2() const = 0; + virtual double GetWeight() const = 0; + virtual ImU32 GetColor() const = 0; + virtual int GetZOrder() const = 0; +}; + +class Canvas2DModel : public Model { + public: + virtual std::span GetLines() = 0; +}; + +void DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize); +void DisplayCanvas2DSettings(Canvas2DModel* model); + +class Canvas2DView : public View { + public: + explicit Canvas2DView(Canvas2DModel* model) : m_model{model} {} + + void Display() override; + void Settings() override; + bool HasSettings() override; + + private: + Canvas2DModel* m_model; +}; +} diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp new file mode 100644 index 00000000000..9c01a915cbc --- /dev/null +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -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. + +#include "glass/networktables/NTCanvas2D.h" + +#include +#include "networktables/NetworkTableInstance.h" +#include "ntcore_cpp.h" + +using namespace glass; + +NTCanvas2DModel::NTCanvas2DModel(std::string_view path) + : NTCanvas2DModel(nt::NetworkTableInstance::GetDefault(), path) { +} + +NTCanvas2DModel::NTCanvas2DModel(nt::NetworkTableInstance inst, std::string_view path) + : m_path{fmt::format("{}/", path)}, m_inst{inst}, m_tableSub{inst, {{m_path}}, {.periodic = 0.05, .sendAll = true}} { +} + +NTCanvas2DModel::~NTCanvas2DModel() = default; + +void NTCanvas2DModel::Update() { +} diff --git a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h new file mode 100644 index 00000000000..a169d27df63 --- /dev/null +++ b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h @@ -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. + +#pragma once + +#include "glass/other/Canvas2D.h" +#include "networktables/MultiSubscriber.h" +#include "networktables/NetworkTableListener.h" +#include "wpi/struct/DynamicStruct.h" +#include +#include +#include + +namespace glass { +class NTCanvas2DModel : public Canvas2DModel { +public: + static constexpr const char *kType = "Canvas2d"; + + explicit NTCanvas2DModel(std::string_view path); + NTCanvas2DModel(nt::NetworkTableInstance inst, std::string_view path); + ~NTCanvas2DModel() override; + + const char *GetPath() const { return m_path.c_str(); } + + void Update() override; + bool Exists() override; + bool IsReadOnly() override; + + std::span GetLines() override; + +private: + std::string m_path; + nt::NetworkTableInstance m_inst; + nt::MultiSubscriber m_tableSub; + nt::NetworkTableListenerPoller m_poller; +}; +} // namespace glass diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index a5f88bd3f36..1e82524075a 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -5,14 +5,26 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.glass.Canvas2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; public class MyRobot extends TimedRobot { + Canvas2d canvas2d = new Canvas2d(100, 100); /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override - public void robotInit() {} + public void robotInit() { + SmartDashboard.putData("Canvas2d", canvas2d); + + canvas2d.drawLine(1, 2, 3, 4, 5, new Color8Bit(Color.kRed)); + canvas2d.drawLine(6, 7, 8, 9, 10, new Color8Bit(Color.kBlue)); + canvas2d.finishFrame(); + + } /** This function is run once each time the robot enters autonomous mode. */ @Override @@ -36,5 +48,6 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override - public void robotPeriodic() {} + public void robotPeriodic() { + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java new file mode 100644 index 00000000000..17732978019 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -0,0 +1,77 @@ +package edu.wpi.first.wpilibj.glass; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.NTSendable; +import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Canvas2d implements NTSendable, AutoCloseable { + private final double[] m_dims = new double[2]; + + // TODO: Seperate list of each primitive type, each one with its own NT topic + private final List m_lines = new ArrayList<>(); + + private boolean m_sendableInitialized = false; + private DoubleArrayPublisher m_dimsPub; + private StructArrayPublisher m_linesPub; + + private int m_currentZOrder = 0; + + public Canvas2d(double width, double length) { + m_dims[0] = width; + m_dims[1] = length; + } + + public void draw(CanvasComplexObject2d object) { + object.drawOn(this); + } + + public void clear() { + m_lines.clear(); + } + + public void drawLine(double x1, double y1, double x2, double y2, double thickness, Color8Bit color) { + m_lines.add(new CanvasLine2d(x1, y1, x2, y2, thickness, color, m_currentZOrder++)); + } + + public void finishFrame() { + if (!m_sendableInitialized) { + return; + } + m_linesPub.set(m_lines.toArray(new CanvasLine2d[0])); + } + + @Override + public void initSendable(NTSendableBuilder builder) { + m_sendableInitialized = true; + + builder.setSmartDashboardType("Canvas2d"); + + NetworkTable table = builder.getTable(); + if (m_dimsPub != null) { + m_dimsPub.close(); + } + m_dimsPub = table.getDoubleArrayTopic("dims").publish(); + m_dimsPub.set(m_dims); + + if (m_linesPub != null) { + m_linesPub.close(); + } + m_linesPub = table.getStructArrayTopic("lines", CanvasLine2d.struct).publish(); + } + + @Override + public void close() { + if (m_dimsPub != null) { + m_dimsPub.close(); + } + if (m_linesPub != null) { + m_linesPub.close(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java new file mode 100644 index 00000000000..356224604c3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java @@ -0,0 +1,5 @@ +package edu.wpi.first.wpilibj.glass; + +public abstract class CanvasComplexObject2d { + public abstract void drawOn(Canvas2d canvas); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java new file mode 100644 index 00000000000..4f5df54635a --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java @@ -0,0 +1,78 @@ +package edu.wpi.first.wpilibj.glass; + +import java.nio.ByteBuffer; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class CanvasLine2d { + public final double m_x1; + public final double m_y1; + public final double m_x2; + public final double m_y2; + public final double m_weight; + public final Color8Bit m_color; + public final int m_zOrder; + + public CanvasLine2d(double x1, double y1, double x2, double y2, double weight, Color8Bit color, int zOrder) { + m_x1 = x1; + m_y1 = y1; + m_x2 = x2; + m_y2 = y2; + + m_weight = weight; + m_color = color; + m_zOrder = zOrder; + } + + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasLine2d.class; + } + + @Override + public String getTypeString() { + return "struct:CanvasLine2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 5 + kSizeInt8 * 3; + } + + @Override + public String getSchema() { + return "double x1;double y1;double x2;double y2;double weight;int8 r;int8 g;int8 b; int32 zOrder"; + } + + @Override + public CanvasLine2d unpack(ByteBuffer bb) { + double x1 = bb.getDouble(); + double y1 = bb.getDouble(); + double x2 = bb.getDouble(); + double y2 = bb.getDouble(); + double weight = bb.getDouble(); + int r = bb.get(); + int g = bb.get(); + int b = bb.get(); + int zOrder = bb.getInt(); + return new CanvasLine2d(x1, y1, x2, y2, weight, new Color8Bit(r, g, b), zOrder); + } + + @Override + public void pack(ByteBuffer bb, CanvasLine2d value) { + bb.putDouble(value.m_x1); + bb.putDouble(value.m_y1); + bb.putDouble(value.m_x2); + bb.putDouble(value.m_y2); + bb.putDouble(value.m_weight); + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_color.green); + bb.putInt(value.m_zOrder); + } + } + + public static final AStruct struct = new AStruct(); +} From bc9fcddfa1cba4f20090b9c2e80862fd7b233f5e Mon Sep 17 00:00:00 2001 From: ohowe Date: Sun, 19 Nov 2023 22:37:48 -0700 Subject: [PATCH 02/16] Drawing lines works --- glass/src/app/native/cpp/main.cpp | 9 +- glass/src/lib/native/cpp/other/Canvas2D.cpp | 39 +++++++- .../lib/native/include/glass/other/Canvas2D.h | 32 +++++-- glass/src/libnt/native/cpp/NTCanvas2D.cpp | 95 +++++++++++++++++-- .../native/cpp/NetworkTablesProvider.cpp | 17 ++-- .../native/cpp/StandardNetworkTables.cpp | 62 ++++++++---- .../include/glass/networktables/NTCanvas2D.h | 49 ++++++---- .../networktables/NetworkTablesProvider.h | 13 ++- myRobot/src/main/java/frc/robot/MyRobot.java | 12 ++- .../src/main/native/cpp/HALSimGui.cpp | 5 +- .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 18 ++-- .../wpilibj/glass/CanvasComplexObject2d.java | 4 + .../wpi/first/wpilibj/glass/CanvasLine2d.java | 42 ++++---- 13 files changed, 304 insertions(+), 93 deletions(-) diff --git a/glass/src/app/native/cpp/main.cpp b/glass/src/app/native/cpp/main.cpp index 0b3473c15cd..70ff3fbb617 100644 --- a/glass/src/app/native/cpp/main.cpp +++ b/glass/src/app/native/cpp/main.cpp @@ -223,17 +223,20 @@ int main(int argc, char** argv) { gPlotProvider = std::make_unique( glass::GetStorageRoot().GetChild("Plots")); - gNtProvider = std::make_unique( - glass::GetStorageRoot().GetChild("NetworkTables")); glass::SetStorageName("glass"); glass::SetStorageDir(saveDir.empty() ? gui::GetPlatformSaveFileDir() : saveDir); gPlotProvider->GlobalInit(); gui::AddInit([] { glass::ResetTime(); }); - gNtProvider->GlobalInit(); NtInitialize(); + // TODO: Hack; moved this below so the model is initialized + gNtProvider = std::make_unique( + glass::GetStorageRoot().GetChild("NetworkTables"), + gNetworkTablesModel->GetStructDatabase()); + gNtProvider->GlobalInit(); + glass::AddStandardNetworkTablesViews(*gNtProvider); gui::AddLateExecute([] { gMainMenu.Display(); }); diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp index b0ec52f2dc3..fe51428d63c 100644 --- a/glass/src/lib/native/cpp/other/Canvas2D.cpp +++ b/glass/src/lib/native/cpp/other/Canvas2D.cpp @@ -11,19 +11,50 @@ using namespace glass; namespace gui = wpi::gui; -void glass::DisplayCanvas2D(Canvas2DModel *model, const ImVec2 &contentSize) { +void glass::DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize) { if (contentSize.x <= 0 || contentSize.y <= 0) { return; } - ImGui::InvisibleButton("background", contentSize); + ImVec2 dimensions = model->GetDimensions(); + + float scale; + ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos(); + if (contentSize.x / dimensions.x > contentSize.y / dimensions.y) { + scale = contentSize.y / dimensions.y; + cursorPos.x += (contentSize.x - dimensions.x * scale) / 2; + } else { + scale = contentSize.x / dimensions.x; + cursorPos.y += (contentSize.y - dimensions.y * scale) / 2; + } + auto drawList = ImGui::GetWindowDrawList(); auto lines = model->GetLines(); // TODO: make this traverse multuple lists for (auto line : lines) { - drawList->AddLine(line->GetPoint1(), line->GetPoint2(), line->GetColor(), - line->GetWeight()); + drawList->AddLine(line.GetPoint1() * scale + cursorPos, + line.GetPoint2() * scale + cursorPos, line.GetColor(), + line.GetWeight() * scale); } } + +void glass::DisplayCanvas2DSettings(Canvas2DModel* model) { + // TODO +} + +void Canvas2DView::Display() { + DisplayCanvas2D(m_model, ImGui::GetWindowContentRegionMax() - + ImGui::GetWindowContentRegionMin()); + ; +} + +void Canvas2DView::Settings() { + DisplayCanvas2DSettings(m_model); +} + +bool Canvas2DView::HasSettings() { + // TODO + return false; +} diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index 69ef6a99804..2c225b55d82 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -14,18 +14,34 @@ namespace glass { -class Canvas2DLineModel : public Model { +class Canvas2DLine { public: - virtual ImVec2 GetPoint1() const = 0; - virtual ImVec2 GetPoint2() const = 0; - virtual double GetWeight() const = 0; - virtual ImU32 GetColor() const = 0; - virtual int GetZOrder() const = 0; + Canvas2DLine(ImVec2 point1, ImVec2 point2, float weight, ImU32 color, + int zOrder) + : m_point1{point1}, + m_point2{point2}, + m_weight{weight}, + m_color{color}, + m_zOrder{zOrder} {} + + ImVec2 GetPoint1() const { return m_point1; } + ImVec2 GetPoint2() const { return m_point2; } + float GetWeight() const { return m_weight; } + ImU32 GetColor() const { return m_color; } + int GetZOrder() const { return m_zOrder; } + + private: + ImVec2 m_point1; + ImVec2 m_point2; + float m_weight; + ImU32 m_color; + int m_zOrder; }; class Canvas2DModel : public Model { public: - virtual std::span GetLines() = 0; + virtual std::vector GetLines() const = 0; + virtual ImVec2 GetDimensions() const = 0; }; void DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize); @@ -42,4 +58,4 @@ class Canvas2DView : public View { private: Canvas2DModel* m_model; }; -} +} // namespace glass diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp index 9c01a915cbc..92110e5e9d6 100644 --- a/glass/src/libnt/native/cpp/NTCanvas2D.cpp +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -4,21 +4,100 @@ #include "glass/networktables/NTCanvas2D.h" +#include +#include + #include -#include "networktables/NetworkTableInstance.h" -#include "ntcore_cpp.h" +#include +#include +#include +#include +#include + +#include "glass/other/Canvas2D.h" using namespace glass; -NTCanvas2DModel::NTCanvas2DModel(std::string_view path) - : NTCanvas2DModel(nt::NetworkTableInstance::GetDefault(), path) { -} +NTCanvas2DModel::NTCanvas2DModel(std::string_view path, + wpi::StructDescriptorDatabase& structDatabase) + : NTCanvas2DModel(nt::NetworkTableInstance::GetDefault(), structDatabase, + path) {} -NTCanvas2DModel::NTCanvas2DModel(nt::NetworkTableInstance inst, std::string_view path) - : m_path{fmt::format("{}/", path)}, m_inst{inst}, m_tableSub{inst, {{m_path}}, {.periodic = 0.05, .sendAll = true}} { -} +// TODO: Default value for dims +NTCanvas2DModel::NTCanvas2DModel(nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, + std::string_view path) + : m_path{fmt::format("{}/", path)}, + m_inst{inst}, + m_structDatabase{structDatabase}, + m_dimensionsSub( + inst.GetFloatArrayTopic(fmt::format("{}/dims", path)).Subscribe({})), + m_linesSub(inst.GetRawTopic(fmt::format("{}/lines", path)) + .Subscribe("struct:CanvasLine2d[]", {})) {} NTCanvas2DModel::~NTCanvas2DModel() = default; void NTCanvas2DModel::Update() { + for (auto&& v : m_dimensionsSub.ReadQueue()) { + if (v.value.size() == 2) { + m_dimensions.x = v.value[0]; + m_dimensions.y = v.value[1]; + } else { + m_dimensions.x = 0; + m_dimensions.y = 0; + } + } + + const wpi::StructDescriptor* lineDesc = m_structDatabase.Find("CanvasLine2d"); + if (lineDesc) { + auto* x1Field = lineDesc->FindFieldByName("x1"); + auto* y1Field = lineDesc->FindFieldByName("y1"); + auto* x2Field = lineDesc->FindFieldByName("x2"); + auto* y2Field = lineDesc->FindFieldByName("y2"); + auto* weightField = lineDesc->FindFieldByName("weight"); + auto* zOrderField = lineDesc->FindFieldByName("zOrder"); + auto* redField = lineDesc->FindFieldByName("r"); + auto* greenField = lineDesc->FindFieldByName("g"); + auto* blueField = lineDesc->FindFieldByName("b"); + + for (auto&& v : m_linesSub.ReadQueue()) { + m_lines.clear(); + + std::span raw(v.value); + int amount = raw.size() / lineDesc->GetSize(); + for (int i = 0; i < amount; i++) { + wpi::DynamicStruct asStruct{lineDesc, raw}; + + ImVec2 p1{asStruct.GetFloatField(x1Field), + asStruct.GetFloatField(y1Field)}; + ImVec2 p2{asStruct.GetFloatField(x2Field), + asStruct.GetFloatField(y2Field)}; + float weight = asStruct.GetFloatField(weightField); + ImU32 color = IM_COL32(asStruct.GetUintField(redField), + asStruct.GetUintField(greenField), + asStruct.GetUintField(blueField), 255); + int zOrder = asStruct.GetIntField(zOrderField); + + m_lines.emplace_back(p1, p2, weight, color, zOrder); + + raw = wpi::drop_front(raw, lineDesc->GetSize()); + } + } + } +} + +bool NTCanvas2DModel::Exists() { + return m_dimensionsSub.Exists(); +} + +bool NTCanvas2DModel::IsReadOnly() { + return true; +} + +ImVec2 NTCanvas2DModel::GetDimensions() const { + return m_dimensions; +} + +std::vector NTCanvas2DModel::GetLines() const { + return m_lines; } diff --git a/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp b/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp index 413899ee4d9..59d927c3d5e 100644 --- a/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp +++ b/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp @@ -13,17 +13,22 @@ #include #include "glass/Storage.h" +#include "glass/networktables/NetworkTables.h" using namespace glass; -NetworkTablesProvider::NetworkTablesProvider(Storage& storage) - : NetworkTablesProvider{storage, nt::NetworkTableInstance::GetDefault()} {} +NetworkTablesProvider::NetworkTablesProvider( + Storage& storage, wpi::StructDescriptorDatabase& structDatabase) + : NetworkTablesProvider{storage, structDatabase, + nt::NetworkTableInstance::GetDefault()} {} -NetworkTablesProvider::NetworkTablesProvider(Storage& storage, - nt::NetworkTableInstance inst) +NetworkTablesProvider::NetworkTablesProvider( + Storage& storage, wpi::StructDescriptorDatabase& structDatabase, + nt::NetworkTableInstance inst) : Provider{storage.GetChild("windows")}, m_inst{inst}, m_poller{inst}, + m_structDatabase{structDatabase}, m_typeCache{storage.GetChild("types")} { storage.SetCustomApply([this] { m_listener = m_poller.AddListener({{""}}, nt::EventFlags::kTopic); @@ -170,8 +175,8 @@ void NetworkTablesProvider::Show(ViewEntry* entry, Window* window) { // get or create model if (!entry->modelEntry->model) { - entry->modelEntry->model = - entry->modelEntry->createModel(m_inst, entry->name.c_str()); + entry->modelEntry->model = entry->modelEntry->createModel( + m_inst, m_structDatabase, entry->name.c_str()); } if (!entry->modelEntry->model) { return; diff --git a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp index ef610253e47..093753acc9f 100644 --- a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp +++ b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp @@ -2,6 +2,9 @@ // 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 "glass/networktables/NTCanvas2D.h" #include "glass/networktables/NTCommandScheduler.h" #include "glass/networktables/NTCommandSelector.h" #include "glass/networktables/NTDifferentialDrive.h" @@ -17,13 +20,28 @@ #include "glass/networktables/NTStringChooser.h" #include "glass/networktables/NTSubsystem.h" #include "glass/networktables/NetworkTablesProvider.h" +#include "glass/other/Canvas2D.h" using namespace glass; void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { + provider.Register( + NTCanvas2DModel::kType, + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { + return std::make_unique(inst, structDatabase, path); + }, + [](Window* win, Model* model, const char*) { + win->SetDefaultPos(400, 400); + win->SetDefaultSize(200, 200); + win->SetPadding(0, 0); + return std::make_unique( + static_cast(model)); + }); provider.Register( NTCommandSchedulerModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -34,7 +52,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTCommandSelectorModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -45,7 +64,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTDifferentialDriveModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -56,9 +76,9 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTFMSModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { - return std::make_unique(inst, path); - }, + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, + const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize); return MakeFunctionView( @@ -66,7 +86,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTDigitalInputModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -77,7 +98,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTDigitalOutputModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -88,7 +110,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTField2DModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [=](Window* win, Model* model, const char* path) { @@ -100,7 +123,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTGyroModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char* path) { @@ -110,7 +134,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTMecanumDriveModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -120,7 +145,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTMechanism2DModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [=](Window* win, Model* model, const char* path) { @@ -132,7 +158,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTPIDControllerModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char* path) { @@ -143,7 +170,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTMotorControllerModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char* path) { @@ -154,7 +182,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTStringChooserModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { @@ -165,7 +194,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTSubsystemModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char*) { diff --git a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h index a169d27df63..1f60f96a7d7 100644 --- a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h +++ b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h @@ -4,35 +4,52 @@ #pragma once -#include "glass/other/Canvas2D.h" -#include "networktables/MultiSubscriber.h" -#include "networktables/NetworkTableListener.h" -#include "wpi/struct/DynamicStruct.h" +#include +#include + +#include +#include +#include +#include #include +#include +#include #include -#include +#include + +#include "glass/other/Canvas2D.h" namespace glass { -class NTCanvas2DModel : public Canvas2DModel { -public: - static constexpr const char *kType = "Canvas2d"; - explicit NTCanvas2DModel(std::string_view path); - NTCanvas2DModel(nt::NetworkTableInstance inst, std::string_view path); +class NTCanvas2DModel : public Canvas2DModel { + public: + static constexpr const char* kType = "Canvas2d"; + + explicit NTCanvas2DModel(std::string_view path, + wpi::StructDescriptorDatabase& structDatabase); + NTCanvas2DModel(nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, + std::string_view path); ~NTCanvas2DModel() override; - const char *GetPath() const { return m_path.c_str(); } + const char* GetPath() const { return m_path.c_str(); } void Update() override; bool Exists() override; bool IsReadOnly() override; - std::span GetLines() override; + std::vector GetLines() const override; + ImVec2 GetDimensions() const override; -private: + private: std::string m_path; nt::NetworkTableInstance m_inst; - nt::MultiSubscriber m_tableSub; - nt::NetworkTableListenerPoller m_poller; + wpi::StructDescriptorDatabase& m_structDatabase; + + nt::FloatArraySubscriber m_dimensionsSub; + nt::RawSubscriber m_linesSub; + + ImVec2 m_dimensions; + std::vector m_lines; }; -} // namespace glass +} // namespace glass diff --git a/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h b/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h index cba34cc6693..2841d301678 100644 --- a/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h +++ b/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h @@ -18,6 +18,7 @@ #include "glass/Model.h" #include "glass/Provider.h" +#include "glass/networktables/NetworkTables.h" namespace glass { @@ -28,7 +29,8 @@ struct NTProviderFunctions { using Exists = std::function; using CreateModel = std::function( - nt::NetworkTableInstance inst, const char* path)>; + nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabse, const char* path)>; using ViewExists = std::function; using CreateView = std::function(Window*, Model*, const char* path)>; @@ -43,8 +45,11 @@ class NetworkTablesProvider : private Provider { using Provider::CreateModelFunc; using Provider::CreateViewFunc; - explicit NetworkTablesProvider(Storage& storage); - NetworkTablesProvider(Storage& storage, nt::NetworkTableInstance inst); + NetworkTablesProvider(Storage& storage, + wpi::StructDescriptorDatabase& structDatabse); + NetworkTablesProvider(Storage& storage, + wpi::StructDescriptorDatabase& structDatabse, + nt::NetworkTableInstance inst); /** * Get the NetworkTables instance being used for this provider. @@ -81,6 +86,8 @@ class NetworkTablesProvider : private Provider { nt::NetworkTableListenerPoller m_poller; NT_Listener m_listener{0}; + wpi::StructDescriptorDatabase& m_structDatabase; + // cached mapping from table name to type string Storage& m_typeCache; diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 1e82524075a..ba5118eecbf 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -4,6 +4,9 @@ package frc.robot; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.glass.Canvas2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,10 +23,15 @@ public class MyRobot extends TimedRobot { public void robotInit() { SmartDashboard.putData("Canvas2d", canvas2d); - canvas2d.drawLine(1, 2, 3, 4, 5, new Color8Bit(Color.kRed)); - canvas2d.drawLine(6, 7, 8, 9, 10, new Color8Bit(Color.kBlue)); + canvas2d.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed)); + canvas2d.drawLine(10, 90, 90, 10, 4, new Color8Bit(Color.kBlue)); canvas2d.finishFrame(); + Rotation2d rotation2d = new Rotation2d(1, 2); + + StructPublisher rotation2dPublisher = NetworkTableInstance.getDefault().getStructTopic("Rotation", Rotation2d.struct).publish(); + rotation2dPublisher.set(rotation2d); + } /** This function is run once each time the robot enters autonomous mode. */ diff --git a/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp b/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp index 4d005ad0d49..118ed7b5da9 100644 --- a/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp @@ -8,6 +8,7 @@ #include #include +#include #include using namespace halsimgui; @@ -16,6 +17,7 @@ glass::MainMenuBar HALSimGui::mainMenu; std::unique_ptr HALSimGui::manager; std::unique_ptr HALSimGui::halProvider; std::unique_ptr HALSimGui::ntProvider; +wpi::StructDescriptorDatabase structDatabase; void HALSimGui::GlobalInit() { manager = std::make_unique( @@ -24,8 +26,9 @@ void HALSimGui::GlobalInit() { halProvider = std::make_unique( glass::GetStorageRoot().GetChild("HALProvider")); halProvider->GlobalInit(); + // TODO: Fix this, it should use whatever struct database it has for the treeview but idk where that is ntProvider = std::make_unique( - glass::GetStorageRoot().GetChild("NTProvider")); + glass::GetStorageRoot().GetChild("NTProvider"), structDatabase); ntProvider->GlobalInit(); wpi::gui::AddLateExecute([] { mainMenu.Display(); }); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index 17732978019..b04be81b3c7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -1,9 +1,13 @@ +// 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.wpilibj.glass; import java.util.ArrayList; import java.util.List; -import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.FloatArrayPublisher; import edu.wpi.first.networktables.NTSendable; import edu.wpi.first.networktables.NTSendableBuilder; import edu.wpi.first.networktables.NetworkTable; @@ -11,18 +15,18 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class Canvas2d implements NTSendable, AutoCloseable { - private final double[] m_dims = new double[2]; + private final float[] m_dims = new float[2]; // TODO: Seperate list of each primitive type, each one with its own NT topic private final List m_lines = new ArrayList<>(); private boolean m_sendableInitialized = false; - private DoubleArrayPublisher m_dimsPub; + private FloatArrayPublisher m_dimsPub; private StructArrayPublisher m_linesPub; private int m_currentZOrder = 0; - public Canvas2d(double width, double length) { + public Canvas2d(float width, float length) { m_dims[0] = width; m_dims[1] = length; } @@ -35,7 +39,7 @@ public void clear() { m_lines.clear(); } - public void drawLine(double x1, double y1, double x2, double y2, double thickness, Color8Bit color) { + public void drawLine(float x1, float y1, float x2, float y2, float thickness, Color8Bit color) { m_lines.add(new CanvasLine2d(x1, y1, x2, y2, thickness, color, m_currentZOrder++)); } @@ -49,14 +53,14 @@ public void finishFrame() { @Override public void initSendable(NTSendableBuilder builder) { m_sendableInitialized = true; - + builder.setSmartDashboardType("Canvas2d"); NetworkTable table = builder.getTable(); if (m_dimsPub != null) { m_dimsPub.close(); } - m_dimsPub = table.getDoubleArrayTopic("dims").publish(); + m_dimsPub = table.getFloatArrayTopic("dims").publish(); m_dimsPub.set(m_dims); if (m_linesPub != null) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java index 356224604c3..85b8a21ecfc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java @@ -1,3 +1,7 @@ +// 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.wpilibj.glass; public abstract class CanvasComplexObject2d { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java index 4f5df54635a..50bfdff2ac1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java @@ -1,3 +1,7 @@ +// 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.wpilibj.glass; import java.nio.ByteBuffer; @@ -6,15 +10,15 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class CanvasLine2d { - public final double m_x1; - public final double m_y1; - public final double m_x2; - public final double m_y2; - public final double m_weight; + public final float m_x1; + public final float m_y1; + public final float m_x2; + public final float m_y2; + public final float m_weight; public final Color8Bit m_color; public final int m_zOrder; - public CanvasLine2d(double x1, double y1, double x2, double y2, double weight, Color8Bit color, int zOrder) { + public CanvasLine2d(float x1, float y1, float x2, float y2, float weight, Color8Bit color, int zOrder) { m_x1 = x1; m_y1 = y1; m_x2 = x2; @@ -38,21 +42,21 @@ public String getTypeString() { @Override public int getSize() { - return kSizeDouble * 5 + kSizeInt8 * 3; + return kSizeFloat * 5 + kSizeInt8 * 3 + kSizeInt32; } @Override public String getSchema() { - return "double x1;double y1;double x2;double y2;double weight;int8 r;int8 g;int8 b; int32 zOrder"; + return "float x1;float y1;float x2;float y2;float weight;uint8 r;uint8 g;uint8 b;int32 zOrder"; } @Override public CanvasLine2d unpack(ByteBuffer bb) { - double x1 = bb.getDouble(); - double y1 = bb.getDouble(); - double x2 = bb.getDouble(); - double y2 = bb.getDouble(); - double weight = bb.getDouble(); + float x1 = bb.getFloat(); + float y1 = bb.getFloat(); + float x2 = bb.getFloat(); + float y2 = bb.getFloat(); + float weight = bb.getFloat(); int r = bb.get(); int g = bb.get(); int b = bb.get(); @@ -62,14 +66,14 @@ public CanvasLine2d unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, CanvasLine2d value) { - bb.putDouble(value.m_x1); - bb.putDouble(value.m_y1); - bb.putDouble(value.m_x2); - bb.putDouble(value.m_y2); - bb.putDouble(value.m_weight); + bb.putFloat(value.m_x1); + bb.putFloat(value.m_y1); + bb.putFloat(value.m_x2); + bb.putFloat(value.m_y2); + bb.putFloat(value.m_weight); bb.put((byte) value.m_color.red); - bb.put((byte) value.m_color.blue); bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); bb.putInt(value.m_zOrder); } } From 30fa850de6dab79ff5482cead4fc15a62fa0eea7 Mon Sep 17 00:00:00 2001 From: ohowe Date: Sun, 19 Nov 2023 22:52:02 -0700 Subject: [PATCH 03/16] hopefully fix CI build? --- glass/src/lib/native/cpp/other/Canvas2D.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp index fe51428d63c..b92a1eba02a 100644 --- a/glass/src/lib/native/cpp/other/Canvas2D.cpp +++ b/glass/src/lib/native/cpp/other/Canvas2D.cpp @@ -4,6 +4,8 @@ #include "glass/other/Canvas2D.h" +#include + #define IMGUI_DEFINE_MATH_OPERATORS #include #include @@ -30,7 +32,7 @@ void glass::DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize) { auto drawList = ImGui::GetWindowDrawList(); - auto lines = model->GetLines(); + std::vector lines = model->GetLines(); // TODO: make this traverse multuple lists for (auto line : lines) { From 6f0c705406455e6a46a7a01790751005a9492f33 Mon Sep 17 00:00:00 2001 From: ohowe Date: Sun, 19 Nov 2023 23:36:11 -0700 Subject: [PATCH 04/16] try 2 but I read the full error message --- glass/src/lib/native/include/glass/other/Canvas2D.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index 2c225b55d82..243470b4ded 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #define IMGUI_DEFINE_MATH_OPERATORS #include From d5a5b635ae2f168da0d3b89846965538e665e754 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 27 Nov 2023 22:25:55 -0700 Subject: [PATCH 05/16] Add Circle & Quad. Among other things --- glass/src/lib/native/cpp/other/Canvas2D.cpp | 45 ++++-- .../lib/native/include/glass/other/Canvas2D.h | 91 ++++++++++- glass/src/libnt/native/cpp/NTCanvas2D.cpp | 130 ++++++++++++--- glass/src/libnt/native/cpp/NetworkTables.cpp | 5 +- .../include/glass/networktables/NTCanvas2D.h | 13 +- myRobot/src/main/java/frc/robot/MyRobot.java | 27 ++-- .../src/main/native/cpp/HALSimGui.cpp | 3 +- .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 152 +++++++++++++++++- .../first/wpilibj/glass/CanvasCircle2d.java | 104 ++++++++++++ .../wpilibj/glass/CanvasComplexObject2d.java | 2 +- .../wpi/first/wpilibj/glass/CanvasLine2d.java | 144 ++++++++++------- .../wpi/first/wpilibj/glass/CanvasQuad2d.java | 134 +++++++++++++++ .../native/include/wpi/struct/DynamicStruct.h | 43 +++++ 13 files changed, 779 insertions(+), 114 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp index b92a1eba02a..272d1136635 100644 --- a/glass/src/lib/native/cpp/other/Canvas2D.cpp +++ b/glass/src/lib/native/cpp/other/Canvas2D.cpp @@ -4,6 +4,7 @@ #include "glass/other/Canvas2D.h" +#include #include #define IMGUI_DEFINE_MATH_OPERATORS @@ -13,12 +14,45 @@ using namespace glass; namespace gui = wpi::gui; +void Canvas2DLine::Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const { + drawList->AddLine(cursorPos + (m_point1 * scale), + cursorPos + (m_point2 * scale), m_color, m_weight * scale); +} + +void Canvas2DQuad::Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const { + if (m_fill) { + drawList->AddQuadFilled(cursorPos + (m_point1 * scale), + cursorPos + (m_point2 * scale), + cursorPos + (m_point3 * scale), + cursorPos + (m_point4 * scale), m_color); + } else { + drawList->AddQuad( + cursorPos + (m_point1 * scale), cursorPos + (m_point2 * scale), + cursorPos + (m_point3 * scale), cursorPos + (m_point4 * scale), m_color, + m_weight * scale); + } +} + +void Canvas2DCircle::Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const { + if (m_fill) { + drawList->AddCircleFilled(cursorPos + (m_center * scale), m_radius * scale, + m_color); + } else { + drawList->AddCircle(cursorPos + (m_center * scale), m_radius * scale, + m_color, 0, m_weight * scale); + } +} + void glass::DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize) { if (contentSize.x <= 0 || contentSize.y <= 0) { return; } ImVec2 dimensions = model->GetDimensions(); + ImDrawList* drawList = ImGui::GetWindowDrawList(); float scale; ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos(); @@ -30,15 +64,8 @@ void glass::DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize) { cursorPos.y += (contentSize.y - dimensions.y * scale) / 2; } - auto drawList = ImGui::GetWindowDrawList(); - - std::vector lines = model->GetLines(); - - // TODO: make this traverse multuple lists - for (auto line : lines) { - drawList->AddLine(line.GetPoint1() * scale + cursorPos, - line.GetPoint2() * scale + cursorPos, line.GetColor(), - line.GetWeight() * scale); + for (auto&& element : model->GetElements()) { + element->Draw(scale, cursorPos, drawList); } } diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index 243470b4ded..9a22120b36d 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -4,6 +4,7 @@ #pragma once +#include #include #define IMGUI_DEFINE_MATH_OPERATORS @@ -14,7 +15,21 @@ namespace glass { -class Canvas2DLine { +class Canvas2DElement { + public: + virtual ~Canvas2DElement() = default; + virtual void Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const = 0; + virtual int GetZOrder() const = 0; +}; + +struct Canvas2DElementSort { + bool operator()(const Canvas2DElement* a, const Canvas2DElement* b) const { + return a->GetZOrder() < b->GetZOrder(); + } +}; + +class Canvas2DLine : public Canvas2DElement { public: Canvas2DLine(ImVec2 point1, ImVec2 point2, float weight, ImU32 color, int zOrder) @@ -28,19 +43,89 @@ class Canvas2DLine { ImVec2 GetPoint2() const { return m_point2; } float GetWeight() const { return m_weight; } ImU32 GetColor() const { return m_color; } - int GetZOrder() const { return m_zOrder; } + int GetZOrder() const override { return m_zOrder; } + + void Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const override; + + private: + ImVec2 m_point1; + ImVec2 m_point2; + float m_weight; + ImU32 m_color; + int m_zOrder; +}; + +class Canvas2DQuad : public Canvas2DElement { + public: + Canvas2DQuad(ImVec2 point1, ImVec2 point2, ImVec2 point3, ImVec2 point4, + float weight, bool fill, ImU32 color, int zOrder) + : m_point1{point1}, + m_point2{point2}, + m_point3{point3}, + m_point4{point4}, + m_weight(weight), + m_fill{fill}, + m_color{color}, + m_zOrder{zOrder} {} + + ImVec2 GetPoint1() const { return m_point1; } + ImVec2 GetPoint2() const { return m_point2; } + ImVec2 GetPoint3() const { return m_point3; } + ImVec2 GetPoint4() const { return m_point4; } + float GetWeight() const { return m_weight; } + ImU32 GetColor() const { return m_color; } + bool IsFill() const { return m_fill; } + int GetZOrder() const override { return m_zOrder; } + + void Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const override; private: ImVec2 m_point1; ImVec2 m_point2; + ImVec2 m_point3; + ImVec2 m_point4; + float m_weight; + bool m_fill; + ImU32 m_color; + int m_zOrder; +}; + +class Canvas2DCircle : public Canvas2DElement { + public: + Canvas2DCircle(ImVec2 center, float radius, float weight, bool fill, + ImU32 color, int zOrder) + : m_center{center}, + m_radius{radius}, + m_weight(weight), + m_fill{fill}, + m_color{color}, + m_zOrder{zOrder} {} + + ImVec2 GetCenter() const { return m_center; } + float GetRadius() const { return m_radius; } + float GetWeight() const { return m_weight; } + bool IsFill() const { return m_fill; } + ImU32 GetColor() const { return m_color; } + int GetZOrder() const override { return m_zOrder; } + + void Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const override; + + private: + ImVec2 m_center; + float m_radius; float m_weight; + bool m_fill; ImU32 m_color; int m_zOrder; }; class Canvas2DModel : public Model { public: - virtual std::vector GetLines() const = 0; + virtual std::set GetElements() + const = 0; virtual ImVec2 GetDimensions() const = 0; }; diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp index 92110e5e9d6..9fb03b3a1ad 100644 --- a/glass/src/libnt/native/cpp/NTCanvas2D.cpp +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -30,14 +30,24 @@ NTCanvas2DModel::NTCanvas2DModel(nt::NetworkTableInstance inst, : m_path{fmt::format("{}/", path)}, m_inst{inst}, m_structDatabase{structDatabase}, + m_nameSub( + inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")), m_dimensionsSub( inst.GetFloatArrayTopic(fmt::format("{}/dims", path)).Subscribe({})), m_linesSub(inst.GetRawTopic(fmt::format("{}/lines", path)) - .Subscribe("struct:CanvasLine2d[]", {})) {} + .Subscribe("struct:CanvasLine2d[]", {})), + m_quadsSub(inst.GetRawTopic(fmt::format("{}/quads", path)) + .Subscribe("struct:CanvasQuad2d[]", {})), + m_circlesSub(inst.GetRawTopic(fmt::format("{}/circles", path)) + .Subscribe("struct:CanvasCircle2d[]", {})) {} NTCanvas2DModel::~NTCanvas2DModel() = default; void NTCanvas2DModel::Update() { + for (auto&& v : m_nameSub.ReadQueue()) { + m_name = v.value; + } + for (auto&& v : m_dimensionsSub.ReadQueue()) { if (v.value.size() == 2) { m_dimensions.x = v.value[0]; @@ -55,35 +65,114 @@ void NTCanvas2DModel::Update() { auto* x2Field = lineDesc->FindFieldByName("x2"); auto* y2Field = lineDesc->FindFieldByName("y2"); auto* weightField = lineDesc->FindFieldByName("weight"); - auto* zOrderField = lineDesc->FindFieldByName("zOrder"); auto* redField = lineDesc->FindFieldByName("r"); auto* greenField = lineDesc->FindFieldByName("g"); auto* blueField = lineDesc->FindFieldByName("b"); + auto* opacityField = lineDesc->FindFieldByName("a"); + auto* zOrderField = lineDesc->FindFieldByName("zOrder"); for (auto&& v : m_linesSub.ReadQueue()) { m_lines.clear(); - std::span raw(v.value); - int amount = raw.size() / lineDesc->GetSize(); - for (int i = 0; i < amount; i++) { - wpi::DynamicStruct asStruct{lineDesc, raw}; - - ImVec2 p1{asStruct.GetFloatField(x1Field), - asStruct.GetFloatField(y1Field)}; - ImVec2 p2{asStruct.GetFloatField(x2Field), - asStruct.GetFloatField(y2Field)}; - float weight = asStruct.GetFloatField(weightField); - ImU32 color = IM_COL32(asStruct.GetUintField(redField), - asStruct.GetUintField(greenField), - asStruct.GetUintField(blueField), 255); - int zOrder = asStruct.GetIntField(zOrderField); + wpi::DynamicStructArray lineArray{lineDesc, v.value}; + for (const auto& line : lineArray) { + ImVec2 point1{line.GetFloatField(x1Field), line.GetFloatField(y1Field)}; + ImVec2 point2{line.GetFloatField(x2Field), line.GetFloatField(y2Field)}; + float weight = line.GetFloatField(weightField); + ImU32 color = IM_COL32( + line.GetUintField(redField), line.GetUintField(greenField), + line.GetUintField(blueField), line.GetUintField(opacityField)); + int zOrder = line.GetIntField(zOrderField); + + m_lines.emplace_back(point1, point2, weight, color, zOrder); + } + } + } - m_lines.emplace_back(p1, p2, weight, color, zOrder); + const wpi::StructDescriptor* quadDesc = m_structDatabase.Find("CanvasQuad2d"); + if (quadDesc) { + auto* x1Field = quadDesc->FindFieldByName("x1"); + auto* y1Field = quadDesc->FindFieldByName("y1"); + auto* x2Field = quadDesc->FindFieldByName("x2"); + auto* y2Field = quadDesc->FindFieldByName("y2"); + auto* x3Field = quadDesc->FindFieldByName("x3"); + auto* y3Field = quadDesc->FindFieldByName("y3"); + auto* x4Field = quadDesc->FindFieldByName("x4"); + auto* y4Field = quadDesc->FindFieldByName("y4"); + auto* weightField = quadDesc->FindFieldByName("weight"); + auto* fillField = quadDesc->FindFieldByName("fill"); + auto* redField = quadDesc->FindFieldByName("r"); + auto* greenField = quadDesc->FindFieldByName("g"); + auto* blueField = quadDesc->FindFieldByName("b"); + auto* opacityField = quadDesc->FindFieldByName("a"); + auto* zOrderField = quadDesc->FindFieldByName("zOrder"); + + for (auto&& v : m_quadsSub.ReadQueue()) { + m_quads.clear(); + + wpi::DynamicStructArray quadArray{quadDesc, v.value}; + for (const auto& quad : quadArray) { + ImVec2 point1{quad.GetFloatField(x1Field), quad.GetFloatField(y1Field)}; + ImVec2 point2{quad.GetFloatField(x2Field), quad.GetFloatField(y2Field)}; + ImVec2 point3{quad.GetFloatField(x3Field), quad.GetFloatField(y3Field)}; + ImVec2 point4{quad.GetFloatField(x4Field), quad.GetFloatField(y4Field)}; + float weight = quad.GetFloatField(weightField); + ImU32 color = IM_COL32( + quad.GetUintField(redField), quad.GetUintField(greenField), + quad.GetUintField(blueField), quad.GetUintField(opacityField)); + bool fill = quad.GetBoolField(fillField); + int zOrder = quad.GetIntField(zOrderField); + + m_quads.emplace_back(point1, point2, point3, point4, weight, fill, + color, zOrder); + } + } + } - raw = wpi::drop_front(raw, lineDesc->GetSize()); + const wpi::StructDescriptor* circleDesc = + m_structDatabase.Find("CanvasCircle2d"); + if (circleDesc) { + auto* xField = circleDesc->FindFieldByName("x"); + auto* yField = circleDesc->FindFieldByName("y"); + auto* radiusField = circleDesc->FindFieldByName("radius"); + auto* weightField = circleDesc->FindFieldByName("weight"); + auto* fillField = circleDesc->FindFieldByName("fill"); + auto* redField = circleDesc->FindFieldByName("r"); + auto* greenField = circleDesc->FindFieldByName("g"); + auto* blueField = circleDesc->FindFieldByName("b"); + auto* opacityField = circleDesc->FindFieldByName("a"); + auto* zOrderField = circleDesc->FindFieldByName("zOrder"); + + for (auto&& v : m_circlesSub.ReadQueue()) { + m_circles.clear(); + + wpi::DynamicStructArray circleArray{circleDesc, v.value}; + for (const auto& circle : circleArray) { + ImVec2 center{circle.GetFloatField(xField), + circle.GetFloatField(yField)}; + float radius = circle.GetFloatField(radiusField); + float weight = circle.GetFloatField(weightField); + bool fill = circle.GetBoolField(fillField); + ImU32 color = IM_COL32( + circle.GetUintField(redField), circle.GetUintField(greenField), + circle.GetUintField(blueField), circle.GetUintField(opacityField)); + int zOrder = circle.GetIntField(zOrderField); + + m_circles.emplace_back(center, radius, weight, fill, color, zOrder); } } } + + m_elements.clear(); + for (const auto& line : m_lines) { + m_elements.insert(&line); + } + for (const auto& quad : m_quads) { + m_elements.insert(&quad); + } + for (const auto& circle : m_circles) { + m_elements.insert(&circle); + } } bool NTCanvas2DModel::Exists() { @@ -98,6 +187,7 @@ ImVec2 NTCanvas2DModel::GetDimensions() const { return m_dimensions; } -std::vector NTCanvas2DModel::GetLines() const { - return m_lines; +std::set +NTCanvas2DModel::GetElements() const { + return m_elements; } diff --git a/glass/src/libnt/native/cpp/NetworkTables.cpp b/glass/src/libnt/native/cpp/NetworkTables.cpp index 57469a20bdc..6fdba31b0c0 100644 --- a/glass/src/libnt/native/cpp/NetworkTables.cpp +++ b/glass/src/libnt/native/cpp/NetworkTables.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include "glass/Context.h" #include "glass/DataSource.h" @@ -740,6 +741,7 @@ void NetworkTablesModel::ValueSource::UpdateFromValue( valueChildrenMap = false; } auto raw = value.GetRaw(); + wpi::DynamicStructArray sArray{desc, raw}; valueChildren.resize(raw.size() / desc->GetSize()); unsigned int i = 0; for (auto&& child : valueChildren) { @@ -747,11 +749,10 @@ void NetworkTablesModel::ValueSource::UpdateFromValue( child.name = fmt::format("[{}]", i); child.path = fmt::format("{}{}", name, child.name); } - wpi::DynamicStruct s{desc, raw}; + wpi::DynamicStruct& s = sArray[i]; UpdateStructValueSource(model, &child, s, child.path, value.last_change()); ++i; - raw = wpi::drop_front(raw, desc->GetSize()); } } else { wpi::DynamicStruct s{desc, value.GetRaw()}; diff --git a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h index 1f60f96a7d7..8d18f7ddede 100644 --- a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h +++ b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -33,12 +34,14 @@ class NTCanvas2DModel : public Canvas2DModel { ~NTCanvas2DModel() override; const char* GetPath() const { return m_path.c_str(); } + const char* GetName() const { return m_name.c_str(); } void Update() override; bool Exists() override; bool IsReadOnly() override; - std::vector GetLines() const override; + std::set GetElements() + const override; ImVec2 GetDimensions() const override; private: @@ -46,10 +49,18 @@ class NTCanvas2DModel : public Canvas2DModel { nt::NetworkTableInstance m_inst; wpi::StructDescriptorDatabase& m_structDatabase; + nt::StringSubscriber m_nameSub; nt::FloatArraySubscriber m_dimensionsSub; nt::RawSubscriber m_linesSub; + nt::RawSubscriber m_quadsSub; + nt::RawSubscriber m_circlesSub; + std::string m_name; ImVec2 m_dimensions; + + std::set m_elements; std::vector m_lines; + std::vector m_quads; + std::vector m_circles; }; } // namespace glass diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index ba5118eecbf..4b24a0a15b7 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -6,15 +6,17 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.glass.Canvas2d; +import edu.wpi.first.wpilibj.glass.CanvasLine2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; public class MyRobot extends TimedRobot { - Canvas2d canvas2d = new Canvas2d(100, 100); + Canvas2d canvas2d = new Canvas2d(150, 150); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -23,15 +25,21 @@ public class MyRobot extends TimedRobot { public void robotInit() { SmartDashboard.putData("Canvas2d", canvas2d); - canvas2d.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed)); - canvas2d.drawLine(10, 90, 90, 10, 4, new Color8Bit(Color.kBlue)); - canvas2d.finishFrame(); - Rotation2d rotation2d = new Rotation2d(1, 2); - StructPublisher rotation2dPublisher = NetworkTableInstance.getDefault().getStructTopic("Rotation", Rotation2d.struct).publish(); - rotation2dPublisher.set(rotation2d); + StructArrayPublisher rotation2dPublisher = + NetworkTableInstance.getDefault().getStructArrayTopic("a", Rotation2d.struct).publish(); + rotation2dPublisher.set(new Rotation2d[] {rotation2d}); + StructArrayPublisher canvasLine2dPublisher = + NetworkTableInstance.getDefault().getStructArrayTopic("b", CanvasLine2d.struct).publish(); + canvasLine2dPublisher.set( + new CanvasLine2d[] {new CanvasLine2d(1, 2, 3, 4, 5, new Color8Bit(Color.kRed), 255, 0)}); + + canvas2d.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed), 255); + canvas2d.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); + canvas2d.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); + canvas2d.finishFrame(); } /** This function is run once each time the robot enters autonomous mode. */ @@ -56,6 +64,5 @@ public void testPeriodic() {} /** This function is called periodically during all modes. */ @Override - public void robotPeriodic() { - } + public void robotPeriodic() {} } diff --git a/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp b/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp index 118ed7b5da9..298981b50f7 100644 --- a/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp @@ -26,7 +26,8 @@ void HALSimGui::GlobalInit() { halProvider = std::make_unique( glass::GetStorageRoot().GetChild("HALProvider")); halProvider->GlobalInit(); - // TODO: Fix this, it should use whatever struct database it has for the treeview but idk where that is + // TODO: Fix this, it should use whatever struct database it has for the + // treeview but idk where that is ntProvider = std::make_unique( glass::GetStorageRoot().GetChild("NTProvider"), structDatabase); ntProvider->GlobalInit(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index b04be81b3c7..bfadc240204 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -4,50 +4,172 @@ package edu.wpi.first.wpilibj.glass; -import java.util.ArrayList; -import java.util.List; - import edu.wpi.first.networktables.FloatArrayPublisher; import edu.wpi.first.networktables.NTSendable; import edu.wpi.first.networktables.NTSendableBuilder; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.util.Color8Bit; +import java.util.ArrayList; +import java.util.List; public class Canvas2d implements NTSendable, AutoCloseable { private final float[] m_dims = new float[2]; - // TODO: Seperate list of each primitive type, each one with its own NT topic private final List m_lines = new ArrayList<>(); + private final List m_quads = new ArrayList<>(); + private final List m_circles = new ArrayList<>(); private boolean m_sendableInitialized = false; private FloatArrayPublisher m_dimsPub; private StructArrayPublisher m_linesPub; + private StructArrayPublisher m_quadsPub; + private StructArrayPublisher m_circlesPub; private int m_currentZOrder = 0; + /** + * Constructs a Canvas2d. + * + * @param width The width of the canvas + * @param length The length of the canvas + */ public Canvas2d(float width, float length) { m_dims[0] = width; m_dims[1] = length; } + /** + * Draw a complex object on the canvas. + * + * @param object The object + */ public void draw(CanvasComplexObject2d object) { object.drawOn(this); } + /** Clears the canvas. */ public void clear() { + m_currentZOrder = 0; m_lines.clear(); + m_quads.clear(); + m_circles.clear(); + } + + /** + * Draws a line on the canvas. + * + * @param x1 The x coordinate of the first point + * @param y1 The y coordinate of the first point + * @param x2 The x coordinate of the second point + * @param y2 The y coordinate of the second point + * @param weight The thickness of the line + * @param color The color of the line + * @param opacity The opacity of the line [0-255] + */ + public void drawLine( + float x1, float y1, float x2, float y2, float thickness, Color8Bit color, int opacity) { + m_lines.add(new CanvasLine2d(x1, y1, x2, y2, thickness, color, opacity, m_currentZOrder++)); + } + + /** + * Draws a rectangle on the canvas. + * + * @param x1 The x coordinate of the first point + * @param y1 The y coordinate of the first point + * @param x2 The x coordinate of the second point + * @param y2 The y coordinate of the second point + * @param thickness The thickness of the outline. For unfilled rectangles. + * @param fill Whether the rectangle should be filled + * @param color The color of the rectangle + * @param opacity The opacity of the rectangle [0-255] + */ + public void drawRect( + float x1, + float y1, + float x2, + float y2, + float thickness, + boolean fill, + Color8Bit color, + int opacity) { + m_quads.add( + new CanvasQuad2d( + x1, y1, x2, y1, x2, y2, x1, y2, thickness, fill, color, opacity, m_currentZOrder++)); + } + + /** + * Draws a quad on the canvas. + * + * @param x1 The x coordinate of the first point + * @param y1 The y coordinate of the first point + * @param x2 The x coordinate of the second point + * @param y2 The y coordinate of the second point + * @param x3 The x coordinate of the third point + * @param y3 The y coordinate of the third point + * @param x4 The x coordinate of the fourth point + * @param y4 The y coordinate of the fourth point + * @param weight The thickness of the outline. For unfilled quads. + * @param fill Whether the quad should be filled + * @param color The color of the quad + * @param opacity The opacity of the quad [0-255] + */ + public void drawQuad( + float x1, + float y1, + float x2, + float y2, + float x3, + float y3, + float x4, + float y4, + float thickness, + boolean fill, + Color8Bit color, + int opacity) { + m_quads.add( + new CanvasQuad2d( + x1, y1, x2, y2, x3, y3, x4, y4, thickness, fill, color, opacity, m_currentZOrder++)); } - public void drawLine(float x1, float y1, float x2, float y2, float thickness, Color8Bit color) { - m_lines.add(new CanvasLine2d(x1, y1, x2, y2, thickness, color, m_currentZOrder++)); + /** + * Draws a circle on the canvas. + * + * @param x The x coordinate of the center of the circle + * @param y The y coordinate of the center of the circle + * @param radius The radius of the circle + * @param weight The thickness of the circle. For unfilled circles. + * @param fill Whether the circle should be filled + * @param color The color of the circle + * @param opacity The opacity of the circle [0-255] + */ + public void drawCircle( + float x, float y, float radius, float thickness, boolean fill, Color8Bit color, int opacity) { + m_circles.add( + new CanvasCircle2d(x, y, radius, thickness, fill, color, opacity, m_currentZOrder++)); } + /** Finish and push the frame to Sendable. Clears the canvas after pushing the frame. */ public void finishFrame() { + finishFrame(true); + } + + /** + * Finish and push the frame to Sendable. + * + * @param clearCanvas Whether to clear the canvas after pushing the frame. + */ + public void finishFrame(boolean clearCanvas) { if (!m_sendableInitialized) { return; } m_linesPub.set(m_lines.toArray(new CanvasLine2d[0])); + m_quadsPub.set(m_quads.toArray(new CanvasQuad2d[0])); + m_circlesPub.set(m_circles.toArray(new CanvasCircle2d[0])); + + if (clearCanvas) { + clear(); + } } @Override @@ -67,6 +189,18 @@ public void initSendable(NTSendableBuilder builder) { m_linesPub.close(); } m_linesPub = table.getStructArrayTopic("lines", CanvasLine2d.struct).publish(); + m_linesPub.set( + new CanvasLine2d[] {new CanvasLine2d(0, 0, 50, 50, 1, new Color8Bit(255, 0, 0), 255, 0)}); + + if (m_quadsPub != null) { + m_quadsPub.close(); + } + m_quadsPub = table.getStructArrayTopic("quads", CanvasQuad2d.struct).publish(); + + if (m_circlesPub != null) { + m_circlesPub.close(); + } + m_circlesPub = table.getStructArrayTopic("circles", CanvasCircle2d.struct).publish(); } @Override @@ -77,5 +211,11 @@ public void close() { if (m_linesPub != null) { m_linesPub.close(); } + if (m_quadsPub != null) { + m_quadsPub.close(); + } + if (m_circlesPub != null) { + m_circlesPub.close(); + } } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java new file mode 100644 index 00000000000..6ad18f4ba43 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java @@ -0,0 +1,104 @@ +// 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.wpilibj.glass; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.util.Color8Bit; +import java.nio.ByteBuffer; + +public class CanvasCircle2d { + public final float m_x; + public final float m_y; + public final float m_radius; + public final float m_weight; + public final boolean m_fill; + public final Color8Bit m_color; + public final int m_opacity; + public final int m_zOrder; + + /** + * Constructs a CanvasCircle2d. + * + * @param x The x coordinate of the center of the circle + * @param y The y coordinate of the center of the circle + * @param radius The radius of the circle + * @param weight The thickness of the circle. For unfilled circles. + * @param fill Whether the circle should be filled + * @param color The color of the circle + * @param opacity The opacity of the circle [0-255] + * @param zOrder The z-order of the circle + */ + public CanvasCircle2d( + float x, + float y, + float radius, + float weight, + boolean fill, + Color8Bit color, + int opacity, + int zOrder) { + m_x = x; + m_y = y; + m_radius = radius; + + m_weight = weight; + m_fill = fill; + m_color = color; + m_opacity = MathUtil.clamp(opacity, 0, 255); + m_zOrder = zOrder; + } + + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasCircle2d.class; + } + + @Override + public String getTypeString() { + return "struct:CanvasCircle2d"; + } + + @Override + public int getSize() { + return kSizeFloat * 4 + kSizeBool + kSizeInt8 * 4 + kSizeInt32; + } + + @Override + public String getSchema() { + return "float x;float y;float radius;float weight;bool fill;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + } + + @Override + public CanvasCircle2d unpack(ByteBuffer bb) { + float x = bb.getFloat(); + float y = bb.getFloat(); + float radius = bb.getFloat(); + float weight = bb.getFloat(); + boolean fill = bb.get() != 0; + Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); + int opacity = bb.get(); + int zOrder = bb.getInt(); + return new CanvasCircle2d(x, y, radius, weight, fill, color, opacity, zOrder); + } + + @Override + public void pack(ByteBuffer bb, CanvasCircle2d value) { + bb.putFloat(value.m_x); + bb.putFloat(value.m_y); + bb.putFloat(value.m_radius); + bb.putFloat(value.m_weight); + bb.put((byte) (value.m_fill ? 1 : 0)); + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_opacity); + bb.putInt(value.m_zOrder); + } + } + + public static final AStruct struct = new AStruct(); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java index 85b8a21ecfc..c7fa21b4ce8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java @@ -5,5 +5,5 @@ package edu.wpi.first.wpilibj.glass; public abstract class CanvasComplexObject2d { - public abstract void drawOn(Canvas2d canvas); + public abstract void drawOn(Canvas2d canvas); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java index 50bfdff2ac1..63816a5b64a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java @@ -4,79 +4,101 @@ package edu.wpi.first.wpilibj.glass; -import java.nio.ByteBuffer; - +import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.struct.Struct; import edu.wpi.first.wpilibj.util.Color8Bit; +import java.nio.ByteBuffer; public class CanvasLine2d { - public final float m_x1; - public final float m_y1; - public final float m_x2; - public final float m_y2; - public final float m_weight; - public final Color8Bit m_color; - public final int m_zOrder; + public final float m_x1; + public final float m_y1; + public final float m_x2; + public final float m_y2; + public final float m_weight; + public final Color8Bit m_color; + public final int m_opacity; + public final int m_zOrder; - public CanvasLine2d(float x1, float y1, float x2, float y2, float weight, Color8Bit color, int zOrder) { - m_x1 = x1; - m_y1 = y1; - m_x2 = x2; - m_y2 = y2; + /** + * Constructs a CanvasLine2d. + * + * @param x1 The x coordinate of the first point + * @param y1 The y coordinate of the first point + * @param x2 The x coordinate of the second point + * @param y2 The y coordinate of the second point + * @param weight The thickness of the line + * @param color The color of the line + * @param opacity The opacity of the line [0-255] + * @param zOrder The z-order of the line + */ + public CanvasLine2d( + float x1, + float y1, + float x2, + float y2, + float weight, + Color8Bit color, + int opacity, + int zOrder) { + m_x1 = x1; + m_y1 = y1; + m_x2 = x2; + m_y2 = y2; - m_weight = weight; - m_color = color; - m_zOrder = zOrder; - } + m_weight = weight; + m_color = color; + m_opacity = MathUtil.clamp(opacity, 0, 255); + m_zOrder = zOrder; + } - public static class AStruct implements Struct { - @Override - public Class getTypeClass() { - return CanvasLine2d.class; - } + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasLine2d.class; + } - @Override - public String getTypeString() { - return "struct:CanvasLine2d"; - } + @Override + public String getTypeString() { + return "struct:CanvasLine2d"; + } - @Override - public int getSize() { - return kSizeFloat * 5 + kSizeInt8 * 3 + kSizeInt32; - } + @Override + public int getSize() { + return kSizeFloat * 5 + kSizeInt8 * 4 + kSizeInt32; + } - @Override - public String getSchema() { - return "float x1;float y1;float x2;float y2;float weight;uint8 r;uint8 g;uint8 b;int32 zOrder"; - } + @Override + public String getSchema() { + return "float x1;float y1;float x2;float y2;float weight;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + } - @Override - public CanvasLine2d unpack(ByteBuffer bb) { - float x1 = bb.getFloat(); - float y1 = bb.getFloat(); - float x2 = bb.getFloat(); - float y2 = bb.getFloat(); - float weight = bb.getFloat(); - int r = bb.get(); - int g = bb.get(); - int b = bb.get(); - int zOrder = bb.getInt(); - return new CanvasLine2d(x1, y1, x2, y2, weight, new Color8Bit(r, g, b), zOrder); - } + @Override + public CanvasLine2d unpack(ByteBuffer bb) { + float x1 = bb.getFloat(); + float y1 = bb.getFloat(); + float x2 = bb.getFloat(); + float y2 = bb.getFloat(); + float weight = bb.getFloat(); + Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); + int opacity = bb.get(); + int zOrder = bb.getInt(); + return new CanvasLine2d(x1, y1, x2, y2, weight, color, opacity, zOrder); + } - @Override - public void pack(ByteBuffer bb, CanvasLine2d value) { - bb.putFloat(value.m_x1); - bb.putFloat(value.m_y1); - bb.putFloat(value.m_x2); - bb.putFloat(value.m_y2); - bb.putFloat(value.m_weight); - bb.put((byte) value.m_color.red); - bb.put((byte) value.m_color.green); - bb.put((byte) value.m_color.blue); - bb.putInt(value.m_zOrder); - } + @Override + public void pack(ByteBuffer bb, CanvasLine2d value) { + bb.putFloat(value.m_x1); + bb.putFloat(value.m_y1); + bb.putFloat(value.m_x2); + bb.putFloat(value.m_y2); + bb.putFloat(value.m_weight); + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_opacity); + bb.putInt(value.m_zOrder); } + } - public static final AStruct struct = new AStruct(); + public static final AStruct struct = new AStruct(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java new file mode 100644 index 00000000000..c62bf9d4352 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java @@ -0,0 +1,134 @@ +// 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.wpilibj.glass; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.util.Color8Bit; +import java.nio.ByteBuffer; + +public class CanvasQuad2d { + public final float m_x1; + public final float m_y1; + public final float m_x2; + public final float m_y2; + public final float m_x3; + public final float m_y3; + public final float m_x4; + public final float m_y4; + public final float m_weight; + public final boolean m_fill; + public final Color8Bit m_color; + public final int m_opacity; + public final int m_zOrder; + + /** + * Constructs a CanvasQuad2d. + * + * @param x1 The x coordinate of the first point + * @param y1 The y coordinate of the first point + * @param x2 The x coordinate of the second point + * @param y2 The y coordinate of the second point + * @param x3 The x coordinate of the third point + * @param y3 The y coordinate of the third point + * @param x4 The x coordinate of the fourth point + * @param y4 The y coordinate of the fourth point + * @param weight The thickness of the outline. For unfilled quads. + * @param fill Whether the quad should be filled + * @param color The color of the quad + * @param opacity The opacity of the quad [0-255] + * @param zOrder The z-order of the quad + */ + public CanvasQuad2d( + float x1, + float y1, + float x2, + float y2, + float x3, + float y3, + float x4, + float y4, + float weight, + boolean fill, + Color8Bit color, + int opacity, + int zOrder) { + m_x1 = x1; + m_y1 = y1; + m_x2 = x2; + m_y2 = y2; + m_x3 = x3; + m_y3 = y3; + m_x4 = x4; + m_y4 = y4; + + m_weight = weight; + m_fill = fill; + m_color = color; + m_opacity = MathUtil.clamp(opacity, 0, 255); + m_zOrder = zOrder; + } + + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasQuad2d.class; + } + + @Override + public String getTypeString() { + return "struct:CanvasQuad2d"; + } + + @Override + public int getSize() { + return kSizeFloat * 9 + kSizeBool + kSizeInt8 * 4 + kSizeInt32; + } + + @Override + public String getSchema() { + return "float x1;float y1;float x2;float y2;float x3;float y3;float x4;float y4;float weight;bool fill;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + } + + @Override + public CanvasQuad2d unpack(ByteBuffer bb) { + float x1 = bb.getFloat(); + float y1 = bb.getFloat(); + float x2 = bb.getFloat(); + float y2 = bb.getFloat(); + float x3 = bb.getFloat(); + float y3 = bb.getFloat(); + float x4 = bb.getFloat(); + float y4 = bb.getFloat(); + float weight = bb.getFloat(); + boolean fill = bb.get() != 0; + Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); + int opacity = bb.get(); + int zOrder = bb.getInt(); + return new CanvasQuad2d(x1, y1, x2, y2, x3, y3, x4, y4, weight, fill, color, opacity, zOrder); + } + + @Override + public void pack(ByteBuffer bb, CanvasQuad2d value) { + bb.putFloat(value.m_x1); + bb.putFloat(value.m_y1); + bb.putFloat(value.m_x2); + bb.putFloat(value.m_y2); + bb.putFloat(value.m_x3); + bb.putFloat(value.m_y3); + bb.putFloat(value.m_x4); + bb.putFloat(value.m_y4); + bb.putFloat(value.m_weight); + bb.put((byte) (value.m_fill ? 1 : 0)); + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_opacity); + bb.putInt(value.m_zOrder); + } + } + + public static final AStruct struct = new AStruct(); +} diff --git a/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h b/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h index b88894c09d0..a4fcb6a15aa 100644 --- a/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h +++ b/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h @@ -679,4 +679,47 @@ class DynamicStructObject : private impl::DSOData, public MutableDynamicStruct { DynamicStructObject& operator=(DynamicStructObject&&) = delete; }; +// TODO: Make for mutable structs (use templates?) +class DynamicStructArray { + public: + DynamicStructArray(const StructDescriptor* desc, + std::span data) + : m_desc{desc}, m_data{data} { + assert(data.size() % desc->GetSize() == 0); + + size_t count = data.size() / desc->GetSize(); + + m_structData.reserve(count); + for (size_t i = 0; i < count; ++i) { + m_structData.emplace_back( + desc, data.subspan(i * desc->GetSize(), desc->GetSize())); + } + } + + const StructDescriptor* GetDescriptor() const { return m_desc; } + std::span GetData() const { return m_data; } + + size_t size() const { return m_structData.size(); } + + DynamicStruct& operator[](size_t i) { return m_structData[i]; } + const DynamicStruct& operator[](size_t i) const { return m_structData[i]; } + + std::vector::iterator begin() { return m_structData.begin(); } + std::vector::iterator end() { return m_structData.end(); } + + std::vector::const_iterator cbegin() const { + return m_structData.cbegin(); + } + std::vector::const_iterator cend() const { + return m_structData.cend(); + } + + protected: + const StructDescriptor* m_desc; + + private: + std::vector m_structData; + std::span m_data; +}; + } // namespace wpi From b922ed5b517173ebfb4a37edc4b439bf199d11e5 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 27 Nov 2023 22:31:29 -0700 Subject: [PATCH 06/16] Fix some formatting --- glass/src/lib/native/cpp/other/Canvas2D.cpp | 1 - myRobot/src/main/java/frc/robot/MyRobot.java | 12 ++++++------ .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 16 ++++++++-------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp index 272d1136635..9469e40a8d5 100644 --- a/glass/src/lib/native/cpp/other/Canvas2D.cpp +++ b/glass/src/lib/native/cpp/other/Canvas2D.cpp @@ -76,7 +76,6 @@ void glass::DisplayCanvas2DSettings(Canvas2DModel* model) { void Canvas2DView::Display() { DisplayCanvas2D(m_model, ImGui::GetWindowContentRegionMax() - ImGui::GetWindowContentRegionMin()); - ; } void Canvas2DView::Settings() { diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 4b24a0a15b7..a537d2463fd 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class MyRobot extends TimedRobot { - Canvas2d canvas2d = new Canvas2d(150, 150); + Canvas2d canvas2D = new Canvas2d(150, 150); /** * This function is run when the robot is first started up and should be used for any @@ -23,7 +23,7 @@ public class MyRobot extends TimedRobot { */ @Override public void robotInit() { - SmartDashboard.putData("Canvas2d", canvas2d); + SmartDashboard.putData("Canvas2d", canvas2D); Rotation2d rotation2d = new Rotation2d(1, 2); @@ -36,10 +36,10 @@ public void robotInit() { canvasLine2dPublisher.set( new CanvasLine2d[] {new CanvasLine2d(1, 2, 3, 4, 5, new Color8Bit(Color.kRed), 255, 0)}); - canvas2d.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed), 255); - canvas2d.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); - canvas2d.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); - canvas2d.finishFrame(); + canvas2D.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed), 255); + canvas2D.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); + canvas2D.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); + canvas2D.finishFrame(); } /** This function is run once each time the robot enters autonomous mode. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index bfadc240204..11520ed7e54 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -68,8 +68,8 @@ public void clear() { * @param opacity The opacity of the line [0-255] */ public void drawLine( - float x1, float y1, float x2, float y2, float thickness, Color8Bit color, int opacity) { - m_lines.add(new CanvasLine2d(x1, y1, x2, y2, thickness, color, opacity, m_currentZOrder++)); + float x1, float y1, float x2, float y2, float weight, Color8Bit color, int opacity) { + m_lines.add(new CanvasLine2d(x1, y1, x2, y2, weight, color, opacity, m_currentZOrder++)); } /** @@ -89,13 +89,13 @@ public void drawRect( float y1, float x2, float y2, - float thickness, + float weight, boolean fill, Color8Bit color, int opacity) { m_quads.add( new CanvasQuad2d( - x1, y1, x2, y1, x2, y2, x1, y2, thickness, fill, color, opacity, m_currentZOrder++)); + x1, y1, x2, y1, x2, y2, x1, y2, weight, fill, color, opacity, m_currentZOrder++)); } /** @@ -123,13 +123,13 @@ public void drawQuad( float y3, float x4, float y4, - float thickness, + float weight, boolean fill, Color8Bit color, int opacity) { m_quads.add( new CanvasQuad2d( - x1, y1, x2, y2, x3, y3, x4, y4, thickness, fill, color, opacity, m_currentZOrder++)); + x1, y1, x2, y2, x3, y3, x4, y4, weight, fill, color, opacity, m_currentZOrder++)); } /** @@ -144,9 +144,9 @@ public void drawQuad( * @param opacity The opacity of the circle [0-255] */ public void drawCircle( - float x, float y, float radius, float thickness, boolean fill, Color8Bit color, int opacity) { + float x, float y, float radius, float weight, boolean fill, Color8Bit color, int opacity) { m_circles.add( - new CanvasCircle2d(x, y, radius, thickness, fill, color, opacity, m_currentZOrder++)); + new CanvasCircle2d(x, y, radius, weight, fill, color, opacity, m_currentZOrder++)); } /** Finish and push the frame to Sendable. Clears the canvas after pushing the frame. */ From 22db98bbca87cee68a4c1c02dfe70eef378fe9a5 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 27 Nov 2023 22:36:42 -0700 Subject: [PATCH 07/16] Try 2 --- .../native/include/glass/networktables/NTCanvas2D.h | 4 +++- myRobot/src/main/java/frc/robot/MyRobot.java | 12 ++++++------ .../java/edu/wpi/first/wpilibj/glass/Canvas2d.java | 2 +- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h index 8d18f7ddede..36a04b413ea 100644 --- a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h +++ b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h @@ -5,7 +5,9 @@ #pragma once #include -#include +#include +#include +#include #include #include diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index a537d2463fd..8dc6be0330f 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class MyRobot extends TimedRobot { - Canvas2d canvas2D = new Canvas2d(150, 150); + Canvas2d m_canvas2D = new Canvas2d(150, 150); /** * This function is run when the robot is first started up and should be used for any @@ -23,7 +23,7 @@ public class MyRobot extends TimedRobot { */ @Override public void robotInit() { - SmartDashboard.putData("Canvas2d", canvas2D); + SmartDashboard.putData("Canvas2d", m_canvas2D); Rotation2d rotation2d = new Rotation2d(1, 2); @@ -36,10 +36,10 @@ public void robotInit() { canvasLine2dPublisher.set( new CanvasLine2d[] {new CanvasLine2d(1, 2, 3, 4, 5, new Color8Bit(Color.kRed), 255, 0)}); - canvas2D.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed), 255); - canvas2D.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); - canvas2D.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); - canvas2D.finishFrame(); + m_canvas2D.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed), 255); + m_canvas2D.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); + m_canvas2D.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); + m_canvas2D.finishFrame(); } /** This function is run once each time the robot enters autonomous mode. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index 11520ed7e54..33de80efc4f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -79,7 +79,7 @@ public void drawLine( * @param y1 The y coordinate of the first point * @param x2 The x coordinate of the second point * @param y2 The y coordinate of the second point - * @param thickness The thickness of the outline. For unfilled rectangles. + * @param weight The thickness of the outline. For unfilled rectangles. * @param fill Whether the rectangle should be filled * @param color The color of the rectangle * @param opacity The opacity of the rectangle [0-255] From 7c6298cdf43797ca0b14d3a6dba26ad916895c23 Mon Sep 17 00:00:00 2001 From: ohowe Date: Wed, 29 Nov 2023 21:26:40 -0700 Subject: [PATCH 08/16] Add ngon --- glass/src/lib/native/cpp/other/Canvas2D.cpp | 11 ++ .../lib/native/include/glass/other/Canvas2D.h | 33 ++++++ glass/src/libnt/native/cpp/NTCanvas2D.cpp | 44 ++++++- .../include/glass/networktables/NTCanvas2D.h | 2 + myRobot/src/main/java/frc/robot/MyRobot.java | 1 + .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 36 ++++++ .../wpi/first/wpilibj/glass/CanvasNgon2d.java | 107 ++++++++++++++++++ 7 files changed, 232 insertions(+), 2 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp index 9469e40a8d5..0b58f6b707d 100644 --- a/glass/src/lib/native/cpp/other/Canvas2D.cpp +++ b/glass/src/lib/native/cpp/other/Canvas2D.cpp @@ -46,6 +46,17 @@ void Canvas2DCircle::Draw(float scale, const ImVec2& cursorPos, } } +void Canvas2DNgon::Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const { + if (m_fill) { + drawList->AddNgonFilled(cursorPos + (m_center * scale), m_radius * scale, + m_color, m_numSides); + } else { + drawList->AddNgon(cursorPos + (m_center * scale), m_radius * scale, + m_color, m_numSides, m_weight * scale); + } +} + void glass::DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize) { if (contentSize.x <= 0 || contentSize.y <= 0) { return; diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index 9a22120b36d..80062df5a04 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -122,6 +122,39 @@ class Canvas2DCircle : public Canvas2DElement { int m_zOrder; }; +class Canvas2DNgon : public Canvas2DElement { +public: + Canvas2DNgon(ImVec2 center, float radius, int numSides, float weight, + bool fill, ImU32 color, int zOrder) + : m_center{center}, + m_radius{radius}, + m_numSides{numSides}, + m_weight{weight}, + m_fill{fill}, + m_color{color}, + m_zOrder{zOrder} {} + + ImVec2 GetCenter() const { return m_center; } + float GetRadius() const { return m_radius; } + int GetNumSides() const { return m_numSides; } + float GetWeight() const { return m_weight; } + bool IsFill() const { return m_fill; } + ImU32 GetColor() const { return m_color; } + int GetZOrder() const override { return m_zOrder; } + + void Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const override; + +private: + ImVec2 m_center; + float m_radius; + int m_numSides; + float m_weight; + bool m_fill; + ImU32 m_color; + int m_zOrder; +}; + class Canvas2DModel : public Model { public: virtual std::set GetElements() diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp index 9fb03b3a1ad..26b407a2f6b 100644 --- a/glass/src/libnt/native/cpp/NTCanvas2D.cpp +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -39,7 +39,9 @@ NTCanvas2DModel::NTCanvas2DModel(nt::NetworkTableInstance inst, m_quadsSub(inst.GetRawTopic(fmt::format("{}/quads", path)) .Subscribe("struct:CanvasQuad2d[]", {})), m_circlesSub(inst.GetRawTopic(fmt::format("{}/circles", path)) - .Subscribe("struct:CanvasCircle2d[]", {})) {} + .Subscribe("struct:CanvasCircle2d[]", {})), + m_ngonsSub(inst.GetRawTopic(fmt::format("{}/ngons", path)) + .Subscribe("struct:CanvasNgon2d[]", {})) {} NTCanvas2DModel::~NTCanvas2DModel() = default; @@ -163,6 +165,41 @@ void NTCanvas2DModel::Update() { } } + const wpi::StructDescriptor* ngonDesc = m_structDatabase.Find("CanvasNgon2d"); + if (ngonDesc) { + auto* xField = ngonDesc->FindFieldByName("x"); + auto* yField = ngonDesc->FindFieldByName("y"); + auto* radiusField = ngonDesc->FindFieldByName("radius"); + auto* numSidesField = ngonDesc->FindFieldByName("numSides"); + auto* weightField = ngonDesc->FindFieldByName("weight"); + auto* fillField = ngonDesc->FindFieldByName("fill"); + auto* redField = ngonDesc->FindFieldByName("r"); + auto* greenField = ngonDesc->FindFieldByName("g"); + auto* blueField = ngonDesc->FindFieldByName("b"); + auto* opacityField = ngonDesc->FindFieldByName("a"); + auto* zOrderField = ngonDesc->FindFieldByName("zOrder"); + + for (auto&& v : m_ngonsSub.ReadQueue()) { + m_ngons.clear(); + + wpi::DynamicStructArray ngonArray{ngonDesc, v.value}; + for (const auto& ngon : ngonArray) { + ImVec2 center{ngon.GetFloatField(xField), ngon.GetFloatField(yField)}; + float radius = ngon.GetFloatField(radiusField); + int numSides = ngon.GetIntField(numSidesField); + float weight = ngon.GetFloatField(weightField); + bool fill = ngon.GetBoolField(fillField); + ImU32 color = IM_COL32( + ngon.GetUintField(redField), ngon.GetUintField(greenField), + ngon.GetUintField(blueField), ngon.GetUintField(opacityField)); + int zOrder = ngon.GetIntField(zOrderField); + + m_ngons.emplace_back(center, radius, numSides, weight, fill, color, + zOrder); + } + } + } + m_elements.clear(); for (const auto& line : m_lines) { m_elements.insert(&line); @@ -173,10 +210,13 @@ void NTCanvas2DModel::Update() { for (const auto& circle : m_circles) { m_elements.insert(&circle); } + for (const auto& ngon : m_ngons) { + m_elements.insert(&ngon); + } } bool NTCanvas2DModel::Exists() { - return m_dimensionsSub.Exists(); + return m_nameSub.Exists(); } bool NTCanvas2DModel::IsReadOnly() { diff --git a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h index 36a04b413ea..7060ed34f76 100644 --- a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h +++ b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h @@ -56,6 +56,7 @@ class NTCanvas2DModel : public Canvas2DModel { nt::RawSubscriber m_linesSub; nt::RawSubscriber m_quadsSub; nt::RawSubscriber m_circlesSub; + nt::RawSubscriber m_ngonsSub; std::string m_name; ImVec2 m_dimensions; @@ -64,5 +65,6 @@ class NTCanvas2DModel : public Canvas2DModel { std::vector m_lines; std::vector m_quads; std::vector m_circles; + std::vector m_ngons; }; } // namespace glass diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 8dc6be0330f..9116bcb5ee9 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -39,6 +39,7 @@ public void robotInit() { m_canvas2D.drawLine(0, 0, 50, 50, 1, new Color8Bit(Color.kRed), 255); m_canvas2D.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); m_canvas2D.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); + m_canvas2D.drawNgon(120, 120, 10, 5, 1, false, new Color8Bit(Color.kYellow), 255); m_canvas2D.finishFrame(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index 33de80efc4f..10061e6601c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -19,12 +19,14 @@ public class Canvas2d implements NTSendable, AutoCloseable { private final List m_lines = new ArrayList<>(); private final List m_quads = new ArrayList<>(); private final List m_circles = new ArrayList<>(); + private final List m_ngons = new ArrayList<>(); private boolean m_sendableInitialized = false; private FloatArrayPublisher m_dimsPub; private StructArrayPublisher m_linesPub; private StructArrayPublisher m_quadsPub; private StructArrayPublisher m_circlesPub; + private StructArrayPublisher m_ngonsPub; private int m_currentZOrder = 0; @@ -149,6 +151,31 @@ public void drawCircle( new CanvasCircle2d(x, y, radius, weight, fill, color, opacity, m_currentZOrder++)); } + /** + * Draws an ngon on the canvas. + * + * @param x The x coordinate of the center of the ngon + * @param y The y coordinate of the center of the ngon + * @param radius The radius of the ngon + * @param numSides The number of sides of the ngon + * @param weight The thickness of the ngon. For unfilled ngons. + * @param fill Whether the ngon should be filled + * @param color The color of the ngon + * @param opacity The opacity of the ngon [0-255] + */ + public void drawNgon( + float x, + float y, + float radius, + int numSides, + float weight, + boolean fill, + Color8Bit color, + int opacity) { + m_ngons.add( + new CanvasNgon2d(x, y, radius, numSides, weight, fill, color, opacity, m_currentZOrder++)); + } + /** Finish and push the frame to Sendable. Clears the canvas after pushing the frame. */ public void finishFrame() { finishFrame(true); @@ -166,6 +193,7 @@ public void finishFrame(boolean clearCanvas) { m_linesPub.set(m_lines.toArray(new CanvasLine2d[0])); m_quadsPub.set(m_quads.toArray(new CanvasQuad2d[0])); m_circlesPub.set(m_circles.toArray(new CanvasCircle2d[0])); + m_ngonsPub.set(m_ngons.toArray(new CanvasNgon2d[0])); if (clearCanvas) { clear(); @@ -201,6 +229,11 @@ public void initSendable(NTSendableBuilder builder) { m_circlesPub.close(); } m_circlesPub = table.getStructArrayTopic("circles", CanvasCircle2d.struct).publish(); + + if (m_ngonsPub != null) { + m_ngonsPub.close(); + } + m_ngonsPub = table.getStructArrayTopic("ngons", CanvasNgon2d.struct).publish(); } @Override @@ -217,5 +250,8 @@ public void close() { if (m_circlesPub != null) { m_circlesPub.close(); } + if (m_ngonsPub != null) { + m_ngonsPub.close(); + } } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java new file mode 100644 index 00000000000..7ea6f9a4cf8 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java @@ -0,0 +1,107 @@ +package edu.wpi.first.wpilibj.glass; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class CanvasNgon2d { + public final float m_x; + public final float m_y; + public final float m_radius; + public final int m_numSides; + public final float m_weight; + public final boolean m_fill; + public final Color8Bit m_color; + public final int m_opacity; + public final int m_zOrder; + + /** + * Constructs a CanvasNgon2d. + * + * @param x The x coordinate of the center of the ngon + * @param y The y coordinate of the center of the ngon + * @param radius The radius of the ngon + * @param numSides The number of sides of the ngon + * @param weight The thickness of the ngon. For unfilled ngons. + * @param fill Whether the ngon should be filled + * @param color The color of the ngon + * @param opacity The opacity of the ngon [0-255] + * @param zOrder The z-order of the ngon + */ + public CanvasNgon2d( + float x, + float y, + float radius, + int numSides, + float weight, + boolean fill, + Color8Bit color, + int opacity, + int zOrder) { + m_x = x; + m_y = y; + m_radius = radius; + m_numSides = numSides; + + m_weight = weight; + m_fill = fill; + m_color = color; + m_opacity = MathUtil.clamp(opacity, 0, 255); + m_zOrder = zOrder; + } + + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasNgon2d.class; + } + + @Override + public String getTypeString() { + return "struct:CanvasNgon2d"; + } + + @Override + public int getSize() { + return kSizeFloat * 3 + kSizeInt32 + kSizeFloat + kSizeBool + kSizeInt8 * 4 + kSizeInt32; + } + + @Override + public String getSchema() { + return "float x;float y;float radius;int32 numSides;float weight;bool fill;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + } + + @Override + public CanvasNgon2d unpack(ByteBuffer bb) { + float x = bb.getFloat(); + float y = bb.getFloat(); + float radius = bb.getFloat(); + int numSides = bb.getInt(); + float weight = bb.getFloat(); + boolean fill = bb.get() != 0; + Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); + int opacity = bb.get(); + int zOrder = bb.getInt(); + return new CanvasNgon2d(x, y, radius, numSides, weight, fill, color, opacity, zOrder); + } + + @Override + public void pack(ByteBuffer bb, CanvasNgon2d value) { + bb.putFloat(value.m_x); + bb.putFloat(value.m_y); + bb.putFloat(value.m_radius); + bb.putInt(value.m_numSides); + bb.putFloat(value.m_weight); + bb.put((byte) (value.m_fill ? 1 : 0)); + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_opacity); + bb.putInt(value.m_zOrder); + } + } + + public static final AStruct struct = new AStruct(); +} From 4fa505ba4124dbd3b4a817584a92f9b149793706 Mon Sep 17 00:00:00 2001 From: ohowe Date: Sun, 17 Dec 2023 14:09:53 -0700 Subject: [PATCH 09/16] Text and color as array --- glass/src/lib/native/cpp/other/Canvas2D.cpp | 11 +- .../lib/native/include/glass/other/Canvas2D.h | 37 +++- glass/src/libnt/native/cpp/NTCanvas2D.cpp | 77 +++++--- .../include/glass/networktables/NTCanvas2D.h | 2 + myRobot/src/main/java/frc/robot/MyRobot.java | 1 + .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 38 +++- .../first/wpilibj/glass/CanvasCircle2d.java | 3 +- .../wpi/first/wpilibj/glass/CanvasLine2d.java | 3 +- .../wpi/first/wpilibj/glass/CanvasNgon2d.java | 180 +++++++++--------- .../wpi/first/wpilibj/glass/CanvasQuad2d.java | 3 +- .../wpi/first/wpilibj/glass/CanvasText2d.java | 139 ++++++++++++++ 11 files changed, 370 insertions(+), 124 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java diff --git a/glass/src/lib/native/cpp/other/Canvas2D.cpp b/glass/src/lib/native/cpp/other/Canvas2D.cpp index 0b58f6b707d..7fbda624a88 100644 --- a/glass/src/lib/native/cpp/other/Canvas2D.cpp +++ b/glass/src/lib/native/cpp/other/Canvas2D.cpp @@ -52,11 +52,18 @@ void Canvas2DNgon::Draw(float scale, const ImVec2& cursorPos, drawList->AddNgonFilled(cursorPos + (m_center * scale), m_radius * scale, m_color, m_numSides); } else { - drawList->AddNgon(cursorPos + (m_center * scale), m_radius * scale, - m_color, m_numSides, m_weight * scale); + drawList->AddNgon(cursorPos + (m_center * scale), m_radius * scale, m_color, + m_numSides, m_weight * scale); } } +void Canvas2DText::Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const { + drawList->AddText(nullptr, m_fontSize * scale, + cursorPos + (m_position * scale), m_color, m_text.c_str(), + nullptr, m_wrapWidth * scale); +} + void glass::DisplayCanvas2D(Canvas2DModel* model, const ImVec2& contentSize) { if (contentSize.x <= 0 || contentSize.y <= 0) { return; diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index 80062df5a04..bce93960d24 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #define IMGUI_DEFINE_MATH_OPERATORS @@ -123,9 +124,9 @@ class Canvas2DCircle : public Canvas2DElement { }; class Canvas2DNgon : public Canvas2DElement { -public: + public: Canvas2DNgon(ImVec2 center, float radius, int numSides, float weight, - bool fill, ImU32 color, int zOrder) + bool fill, ImU32 color, int zOrder) : m_center{center}, m_radius{radius}, m_numSides{numSides}, @@ -145,7 +146,7 @@ class Canvas2DNgon : public Canvas2DElement { void Draw(float scale, const ImVec2& cursorPos, ImDrawList* drawList) const override; -private: + private: ImVec2 m_center; float m_radius; int m_numSides; @@ -155,6 +156,36 @@ class Canvas2DNgon : public Canvas2DElement { int m_zOrder; }; +class Canvas2DText : public Canvas2DElement { + public: + Canvas2DText(ImVec2 position, float fontSize, float wrapWidth, + std::string text, ImU32 color, int zOrder) + : m_position{position}, + m_fontSize{fontSize}, + m_wrapWidth{wrapWidth}, + m_text(std::move(text)), + m_color{color}, + m_zOrder{zOrder} {} + + ImVec2 GetPosition() const { return m_position; } + float GetSize() const { return m_fontSize; } + float GetWrapWidth() const { return m_wrapWidth; } + ImU32 GetColor() const { return m_color; } + std::string_view GetText() const { return m_text; } + int GetZOrder() const override { return m_zOrder; } + + void Draw(float scale, const ImVec2& cursorPos, + ImDrawList* drawList) const override; + + private: + ImVec2 m_position; + float m_fontSize; + float m_wrapWidth; + std::string m_text; + ImU32 m_color; + int m_zOrder; +}; + class Canvas2DModel : public Model { public: virtual std::set GetElements() diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp index 26b407a2f6b..25b5c4421d6 100644 --- a/glass/src/libnt/native/cpp/NTCanvas2D.cpp +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -41,7 +41,9 @@ NTCanvas2DModel::NTCanvas2DModel(nt::NetworkTableInstance inst, m_circlesSub(inst.GetRawTopic(fmt::format("{}/circles", path)) .Subscribe("struct:CanvasCircle2d[]", {})), m_ngonsSub(inst.GetRawTopic(fmt::format("{}/ngons", path)) - .Subscribe("struct:CanvasNgon2d[]", {})) {} + .Subscribe("struct:CanvasNgon2d[]", {})), + m_textsSub(inst.GetRawTopic(fmt::format("{}/texts", path)) + .Subscribe("struct:CanvasText2d[]", {})) {} NTCanvas2DModel::~NTCanvas2DModel() = default; @@ -67,10 +69,7 @@ void NTCanvas2DModel::Update() { auto* x2Field = lineDesc->FindFieldByName("x2"); auto* y2Field = lineDesc->FindFieldByName("y2"); auto* weightField = lineDesc->FindFieldByName("weight"); - auto* redField = lineDesc->FindFieldByName("r"); - auto* greenField = lineDesc->FindFieldByName("g"); - auto* blueField = lineDesc->FindFieldByName("b"); - auto* opacityField = lineDesc->FindFieldByName("a"); + auto* colorField = lineDesc->FindFieldByName("color"); auto* zOrderField = lineDesc->FindFieldByName("zOrder"); for (auto&& v : m_linesSub.ReadQueue()) { @@ -82,8 +81,8 @@ void NTCanvas2DModel::Update() { ImVec2 point2{line.GetFloatField(x2Field), line.GetFloatField(y2Field)}; float weight = line.GetFloatField(weightField); ImU32 color = IM_COL32( - line.GetUintField(redField), line.GetUintField(greenField), - line.GetUintField(blueField), line.GetUintField(opacityField)); + line.GetUintField(colorField, 0), line.GetUintField(colorField, 1), + line.GetUintField(colorField, 2), line.GetUintField(colorField, 3)); int zOrder = line.GetIntField(zOrderField); m_lines.emplace_back(point1, point2, weight, color, zOrder); @@ -103,10 +102,7 @@ void NTCanvas2DModel::Update() { auto* y4Field = quadDesc->FindFieldByName("y4"); auto* weightField = quadDesc->FindFieldByName("weight"); auto* fillField = quadDesc->FindFieldByName("fill"); - auto* redField = quadDesc->FindFieldByName("r"); - auto* greenField = quadDesc->FindFieldByName("g"); - auto* blueField = quadDesc->FindFieldByName("b"); - auto* opacityField = quadDesc->FindFieldByName("a"); + auto* colorField = quadDesc->FindFieldByName("color"); auto* zOrderField = quadDesc->FindFieldByName("zOrder"); for (auto&& v : m_quadsSub.ReadQueue()) { @@ -120,8 +116,8 @@ void NTCanvas2DModel::Update() { ImVec2 point4{quad.GetFloatField(x4Field), quad.GetFloatField(y4Field)}; float weight = quad.GetFloatField(weightField); ImU32 color = IM_COL32( - quad.GetUintField(redField), quad.GetUintField(greenField), - quad.GetUintField(blueField), quad.GetUintField(opacityField)); + quad.GetUintField(colorField, 0), quad.GetUintField(colorField, 1), + quad.GetUintField(colorField, 2), quad.GetUintField(colorField, 3)); bool fill = quad.GetBoolField(fillField); int zOrder = quad.GetIntField(zOrderField); @@ -139,10 +135,7 @@ void NTCanvas2DModel::Update() { auto* radiusField = circleDesc->FindFieldByName("radius"); auto* weightField = circleDesc->FindFieldByName("weight"); auto* fillField = circleDesc->FindFieldByName("fill"); - auto* redField = circleDesc->FindFieldByName("r"); - auto* greenField = circleDesc->FindFieldByName("g"); - auto* blueField = circleDesc->FindFieldByName("b"); - auto* opacityField = circleDesc->FindFieldByName("a"); + auto* colorField = circleDesc->FindFieldByName("color"); auto* zOrderField = circleDesc->FindFieldByName("zOrder"); for (auto&& v : m_circlesSub.ReadQueue()) { @@ -155,9 +148,10 @@ void NTCanvas2DModel::Update() { float radius = circle.GetFloatField(radiusField); float weight = circle.GetFloatField(weightField); bool fill = circle.GetBoolField(fillField); - ImU32 color = IM_COL32( - circle.GetUintField(redField), circle.GetUintField(greenField), - circle.GetUintField(blueField), circle.GetUintField(opacityField)); + ImU32 color = IM_COL32(circle.GetUintField(colorField, 0), + circle.GetUintField(colorField, 1), + circle.GetUintField(colorField, 2), + circle.GetUintField(colorField, 3)); int zOrder = circle.GetIntField(zOrderField); m_circles.emplace_back(center, radius, weight, fill, color, zOrder); @@ -173,10 +167,7 @@ void NTCanvas2DModel::Update() { auto* numSidesField = ngonDesc->FindFieldByName("numSides"); auto* weightField = ngonDesc->FindFieldByName("weight"); auto* fillField = ngonDesc->FindFieldByName("fill"); - auto* redField = ngonDesc->FindFieldByName("r"); - auto* greenField = ngonDesc->FindFieldByName("g"); - auto* blueField = ngonDesc->FindFieldByName("b"); - auto* opacityField = ngonDesc->FindFieldByName("a"); + auto* colorField = ngonDesc->FindFieldByName("color"); auto* zOrderField = ngonDesc->FindFieldByName("zOrder"); for (auto&& v : m_ngonsSub.ReadQueue()) { @@ -190,8 +181,8 @@ void NTCanvas2DModel::Update() { float weight = ngon.GetFloatField(weightField); bool fill = ngon.GetBoolField(fillField); ImU32 color = IM_COL32( - ngon.GetUintField(redField), ngon.GetUintField(greenField), - ngon.GetUintField(blueField), ngon.GetUintField(opacityField)); + ngon.GetUintField(colorField, 0), ngon.GetUintField(colorField, 1), + ngon.GetUintField(colorField, 2), ngon.GetUintField(colorField, 3)); int zOrder = ngon.GetIntField(zOrderField); m_ngons.emplace_back(center, radius, numSides, weight, fill, color, @@ -200,6 +191,37 @@ void NTCanvas2DModel::Update() { } } + const wpi::StructDescriptor* textDesc = m_structDatabase.Find("CanvasText2d"); + if (textDesc) { + auto* xField = textDesc->FindFieldByName("x"); + auto* yField = textDesc->FindFieldByName("y"); + auto* fontSizeField = textDesc->FindFieldByName("fontSize"); + auto* wrapWidthField = textDesc->FindFieldByName("wrapWidth"); + auto* textField = textDesc->FindFieldByName("text"); + auto* colorField = textDesc->FindFieldByName("color"); + auto* zOrderField = textDesc->FindFieldByName("zOrder"); + + for (auto&& v : m_textsSub.ReadQueue()) { + m_texts.clear(); + + wpi::DynamicStructArray textArray{textDesc, v.value}; + for (const auto& text : textArray) { + ImVec2 position{text.GetFloatField(xField), text.GetFloatField(yField)}; + float fontSize = text.GetFloatField(fontSizeField); + float wrapWidth = text.GetFloatField(wrapWidthField); + ImU32 color = IM_COL32( + text.GetUintField(colorField, 0), text.GetUintField(colorField, 1), + text.GetUintField(colorField, 2), text.GetUintField(colorField, 3)); + std::string textStr{text.GetStringField(textField)}; + int zOrder = text.GetIntField(zOrderField); + + m_texts.emplace_back(position, fontSize, wrapWidth, std::move(textStr), + color, zOrder); + m_texts.size(); + } + } + } + m_elements.clear(); for (const auto& line : m_lines) { m_elements.insert(&line); @@ -213,6 +235,9 @@ void NTCanvas2DModel::Update() { for (const auto& ngon : m_ngons) { m_elements.insert(&ngon); } + for (const auto& text : m_texts) { + m_elements.insert(&text); + } } bool NTCanvas2DModel::Exists() { diff --git a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h index 7060ed34f76..eacd8a85f35 100644 --- a/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h +++ b/glass/src/libnt/native/include/glass/networktables/NTCanvas2D.h @@ -57,6 +57,7 @@ class NTCanvas2DModel : public Canvas2DModel { nt::RawSubscriber m_quadsSub; nt::RawSubscriber m_circlesSub; nt::RawSubscriber m_ngonsSub; + nt::RawSubscriber m_textsSub; std::string m_name; ImVec2 m_dimensions; @@ -66,5 +67,6 @@ class NTCanvas2DModel : public Canvas2DModel { std::vector m_quads; std::vector m_circles; std::vector m_ngons; + std::vector m_texts; }; } // namespace glass diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 9116bcb5ee9..0a114014718 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -40,6 +40,7 @@ public void robotInit() { m_canvas2D.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); m_canvas2D.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); m_canvas2D.drawNgon(120, 120, 10, 5, 1, false, new Color8Bit(Color.kYellow), 255); + m_canvas2D.drawText(5, 5, 5, -1, "Hello world", new Color8Bit(255, 2, 51), 255); m_canvas2D.finishFrame(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index 10061e6601c..c3e49db5bab 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -20,6 +20,7 @@ public class Canvas2d implements NTSendable, AutoCloseable { private final List m_quads = new ArrayList<>(); private final List m_circles = new ArrayList<>(); private final List m_ngons = new ArrayList<>(); + private final List m_texts = new ArrayList<>(); private boolean m_sendableInitialized = false; private FloatArrayPublisher m_dimsPub; @@ -27,6 +28,7 @@ public class Canvas2d implements NTSendable, AutoCloseable { private StructArrayPublisher m_quadsPub; private StructArrayPublisher m_circlesPub; private StructArrayPublisher m_ngonsPub; + private StructArrayPublisher m_textsPub; private int m_currentZOrder = 0; @@ -56,6 +58,8 @@ public void clear() { m_lines.clear(); m_quads.clear(); m_circles.clear(); + m_ngons.clear(); + m_texts.clear(); } /** @@ -176,6 +180,29 @@ public void drawNgon( new CanvasNgon2d(x, y, radius, numSides, weight, fill, color, opacity, m_currentZOrder++)); } + /** + * Draws text on the canvas. + * + * @param x The x coordinate of the text + * @param y The y coordinate of the text + * @param fontSize The size of the text + * @param wrapWidth The width at which the text should wrap. 0 for no wrap + * @param text The text. The max length of the text is 64 characters + * @param color The color of the text + * @param opacity The opacity of the text [0-255] + */ + public void drawText( + float x, + float y, + float fontSize, + float wrapWidth, + String text, + Color8Bit color, + int opacity) { + m_texts.add( + new CanvasText2d(x, y, fontSize, wrapWidth, text, color, opacity, m_currentZOrder++)); + } + /** Finish and push the frame to Sendable. Clears the canvas after pushing the frame. */ public void finishFrame() { finishFrame(true); @@ -194,6 +221,7 @@ public void finishFrame(boolean clearCanvas) { m_quadsPub.set(m_quads.toArray(new CanvasQuad2d[0])); m_circlesPub.set(m_circles.toArray(new CanvasCircle2d[0])); m_ngonsPub.set(m_ngons.toArray(new CanvasNgon2d[0])); + m_textsPub.set(m_texts.toArray(new CanvasText2d[0])); if (clearCanvas) { clear(); @@ -217,8 +245,6 @@ public void initSendable(NTSendableBuilder builder) { m_linesPub.close(); } m_linesPub = table.getStructArrayTopic("lines", CanvasLine2d.struct).publish(); - m_linesPub.set( - new CanvasLine2d[] {new CanvasLine2d(0, 0, 50, 50, 1, new Color8Bit(255, 0, 0), 255, 0)}); if (m_quadsPub != null) { m_quadsPub.close(); @@ -234,6 +260,11 @@ public void initSendable(NTSendableBuilder builder) { m_ngonsPub.close(); } m_ngonsPub = table.getStructArrayTopic("ngons", CanvasNgon2d.struct).publish(); + + if (m_textsPub != null) { + m_textsPub.close(); + } + m_textsPub = table.getStructArrayTopic("texts", CanvasText2d.struct).publish(); } @Override @@ -253,5 +284,8 @@ public void close() { if (m_ngonsPub != null) { m_ngonsPub.close(); } + if (m_textsPub != null) { + m_textsPub.close(); + } } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java index 6ad18f4ba43..58423d3aac1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java @@ -69,7 +69,7 @@ public int getSize() { @Override public String getSchema() { - return "float x;float y;float radius;float weight;bool fill;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + return "float x;float y;float radius;float weight;bool fill;uint8 color[4];int32 zOrder"; } @Override @@ -82,6 +82,7 @@ public CanvasCircle2d unpack(ByteBuffer bb) { Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); int opacity = bb.get(); int zOrder = bb.getInt(); + return new CanvasCircle2d(x, y, radius, weight, fill, color, opacity, zOrder); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java index 63816a5b64a..a58d52347fa 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java @@ -69,7 +69,7 @@ public int getSize() { @Override public String getSchema() { - return "float x1;float y1;float x2;float y2;float weight;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + return "float x1;float y1;float x2;float y2;float weight;uint8 color[4];int32 zOrder"; } @Override @@ -82,6 +82,7 @@ public CanvasLine2d unpack(ByteBuffer bb) { Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); int opacity = bb.get(); int zOrder = bb.getInt(); + return new CanvasLine2d(x1, y1, x2, y2, weight, color, opacity, zOrder); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java index 7ea6f9a4cf8..0ebfe8170ca 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java @@ -1,107 +1,111 @@ -package edu.wpi.first.wpilibj.glass; +// 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. -import java.nio.ByteBuffer; +package edu.wpi.first.wpilibj.glass; import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.struct.Struct; import edu.wpi.first.wpilibj.util.Color8Bit; +import java.nio.ByteBuffer; public class CanvasNgon2d { - public final float m_x; - public final float m_y; - public final float m_radius; - public final int m_numSides; - public final float m_weight; - public final boolean m_fill; - public final Color8Bit m_color; - public final int m_opacity; - public final int m_zOrder; + public final float m_x; + public final float m_y; + public final float m_radius; + public final int m_numSides; + public final float m_weight; + public final boolean m_fill; + public final Color8Bit m_color; + public final int m_opacity; + public final int m_zOrder; + + /** + * Constructs a CanvasNgon2d. + * + * @param x The x coordinate of the center of the ngon + * @param y The y coordinate of the center of the ngon + * @param radius The radius of the ngon + * @param numSides The number of sides of the ngon + * @param weight The thickness of the ngon. For unfilled ngons. + * @param fill Whether the ngon should be filled + * @param color The color of the ngon + * @param opacity The opacity of the ngon [0-255] + * @param zOrder The z-order of the ngon + */ + public CanvasNgon2d( + float x, + float y, + float radius, + int numSides, + float weight, + boolean fill, + Color8Bit color, + int opacity, + int zOrder) { + m_x = x; + m_y = y; + m_radius = radius; + m_numSides = numSides; - /** - * Constructs a CanvasNgon2d. - * - * @param x The x coordinate of the center of the ngon - * @param y The y coordinate of the center of the ngon - * @param radius The radius of the ngon - * @param numSides The number of sides of the ngon - * @param weight The thickness of the ngon. For unfilled ngons. - * @param fill Whether the ngon should be filled - * @param color The color of the ngon - * @param opacity The opacity of the ngon [0-255] - * @param zOrder The z-order of the ngon - */ - public CanvasNgon2d( - float x, - float y, - float radius, - int numSides, - float weight, - boolean fill, - Color8Bit color, - int opacity, - int zOrder) { - m_x = x; - m_y = y; - m_radius = radius; - m_numSides = numSides; + m_weight = weight; + m_fill = fill; + m_color = color; + m_opacity = MathUtil.clamp(opacity, 0, 255); + m_zOrder = zOrder; + } - m_weight = weight; - m_fill = fill; - m_color = color; - m_opacity = MathUtil.clamp(opacity, 0, 255); - m_zOrder = zOrder; + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasNgon2d.class; } - public static class AStruct implements Struct { - @Override - public Class getTypeClass() { - return CanvasNgon2d.class; - } + @Override + public String getTypeString() { + return "struct:CanvasNgon2d"; + } - @Override - public String getTypeString() { - return "struct:CanvasNgon2d"; - } + @Override + public int getSize() { + return kSizeFloat * 3 + kSizeInt32 + kSizeFloat + kSizeBool + kSizeInt8 * 4 + kSizeInt32; + } - @Override - public int getSize() { - return kSizeFloat * 3 + kSizeInt32 + kSizeFloat + kSizeBool + kSizeInt8 * 4 + kSizeInt32; - } + @Override + public String getSchema() { + return "float x;float y;float radius;int32 numSides;float weight;bool fill;uint8 color[4];int32 zOrder"; + } - @Override - public String getSchema() { - return "float x;float y;float radius;int32 numSides;float weight;bool fill;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; - } + @Override + public CanvasNgon2d unpack(ByteBuffer bb) { + float x = bb.getFloat(); + float y = bb.getFloat(); + float radius = bb.getFloat(); + int numSides = bb.getInt(); + float weight = bb.getFloat(); + boolean fill = bb.get() != 0; + Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); + int opacity = bb.get(); + int zOrder = bb.getInt(); - @Override - public CanvasNgon2d unpack(ByteBuffer bb) { - float x = bb.getFloat(); - float y = bb.getFloat(); - float radius = bb.getFloat(); - int numSides = bb.getInt(); - float weight = bb.getFloat(); - boolean fill = bb.get() != 0; - Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); - int opacity = bb.get(); - int zOrder = bb.getInt(); - return new CanvasNgon2d(x, y, radius, numSides, weight, fill, color, opacity, zOrder); - } + return new CanvasNgon2d(x, y, radius, numSides, weight, fill, color, opacity, zOrder); + } - @Override - public void pack(ByteBuffer bb, CanvasNgon2d value) { - bb.putFloat(value.m_x); - bb.putFloat(value.m_y); - bb.putFloat(value.m_radius); - bb.putInt(value.m_numSides); - bb.putFloat(value.m_weight); - bb.put((byte) (value.m_fill ? 1 : 0)); - bb.put((byte) value.m_color.red); - bb.put((byte) value.m_color.green); - bb.put((byte) value.m_color.blue); - bb.put((byte) value.m_opacity); - bb.putInt(value.m_zOrder); - } + @Override + public void pack(ByteBuffer bb, CanvasNgon2d value) { + bb.putFloat(value.m_x); + bb.putFloat(value.m_y); + bb.putFloat(value.m_radius); + bb.putInt(value.m_numSides); + bb.putFloat(value.m_weight); + bb.put((byte) (value.m_fill ? 1 : 0)); + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_opacity); + bb.putInt(value.m_zOrder); } + } - public static final AStruct struct = new AStruct(); + public static final AStruct struct = new AStruct(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java index c62bf9d4352..93cb24c166c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java @@ -89,7 +89,7 @@ public int getSize() { @Override public String getSchema() { - return "float x1;float y1;float x2;float y2;float x3;float y3;float x4;float y4;float weight;bool fill;uint8 r;uint8 g;uint8 b;uint8 a;int32 zOrder"; + return "float x1;float y1;float x2;float y2;float x3;float y3;float x4;float y4;float weight;bool fill;uint8 color[4];int32 zOrder"; } @Override @@ -107,6 +107,7 @@ public CanvasQuad2d unpack(ByteBuffer bb) { Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); int opacity = bb.get(); int zOrder = bb.getInt(); + return new CanvasQuad2d(x1, y1, x2, y2, x3, y3, x4, y4, weight, fill, color, opacity, zOrder); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java new file mode 100644 index 00000000000..6d52a849a7c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java @@ -0,0 +1,139 @@ +// 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.wpilibj.glass; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.util.Color8Bit; +import java.nio.ByteBuffer; + +class CanvasText2d { + public final float m_x; + public final float m_y; + public final float m_fontSize; + public final float m_wrapWidth; + public final char[] m_text; + public final Color8Bit m_color; + public final int m_opacity; + public final int m_zOrder; + + /** + * Constructs a CanvasText2d. + * + * @param x The x coordinate of the text + * @param y The y coordinate of the text + * @param fontSize The size of the text + * @param wrapWidth The width at which the text should wrap. 0 for no wrap + * @param text The text. The max of length the text is 64 characters + * @param color The color of the text + * @param opacity The opacity of the text [0-255] + * @param zOrder The z-order of the text + */ + public CanvasText2d( + float x, + float y, + float fontSize, + float wrapWidth, + String text, + Color8Bit color, + int opacity, + int zOrder) { + this(x, y, fontSize, wrapWidth, text.toCharArray(), color, opacity, zOrder); + } + + /** + * Constructs a CanvasText2d. + * + * @param x The x coordinate of the text + * @param y The y coordinate of the text + * @param size The size of the text + * @param wrapWidth The width at which the text should wrap + * @param text The text. The max of length the text is 64 characters + * @param color The color of the text + * @param opacity The opacity of the text [0-255] + * @param zOrder The z-order of the text + */ + public CanvasText2d( + float x, + float y, + float size, + float wrapWidth, + char[] text, + Color8Bit color, + int opacity, + int zOrder) { + // TODO: check text length + m_x = x; + m_y = y; + m_fontSize = size; + m_wrapWidth = wrapWidth; + m_text = text; + m_color = color; + m_opacity = MathUtil.clamp(opacity, 0, 255); + m_zOrder = zOrder; + } + + public static class AStruct implements Struct { + @Override + public Class getTypeClass() { + return CanvasText2d.class; + } + + @Override + public String getTypeString() { + return "struct:CanvasText2d"; + } + + @Override + public int getSize() { + return kSizeFloat * 4 + kSizeInt8 * 64 + kSizeInt8 * 4 + kSizeInt32; + } + + @Override + public String getSchema() { + return "float x;float y;float fontSize;float wrapWidth;char text[64];uint8 color[4];int32 zOrder"; + } + + @Override + public CanvasText2d unpack(ByteBuffer bb) { + float x = bb.getFloat(); + float y = bb.getFloat(); + float fontSize = bb.getFloat(); + float wrapWidth = bb.getFloat(); + char[] text = new char[64]; + for (int i = 0; i < 64; i++) { + text[i] = (char) bb.get(); + } + Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); + int opacity = bb.get(); + int zOrder = bb.getInt(); + + return new CanvasText2d(x, y, fontSize, wrapWidth, text, color, opacity, zOrder); + } + + @Override + public void pack(ByteBuffer bb, CanvasText2d value) { + System.out.println(bb.position()); + bb.putFloat(value.m_x); + bb.putFloat(value.m_y); + bb.putFloat(value.m_fontSize); + bb.putFloat(value.m_wrapWidth); + for (int i = 0; i < value.m_text.length; i++) { + bb.put((byte) value.m_text[i]); + } + for (int i = value.m_text.length; i < 64; i++) { + bb.put((byte) 0); + } + bb.put((byte) value.m_color.red); + bb.put((byte) value.m_color.green); + bb.put((byte) value.m_color.blue); + bb.put((byte) value.m_opacity); + bb.putInt(value.m_zOrder); + System.out.println(bb.position()); + } + } + + public static final AStruct struct = new AStruct(); +} From 8bbf6a754a67167cf8ed72bc463d46bddf65eb31 Mon Sep 17 00:00:00 2001 From: ohowe Date: Wed, 20 Dec 2023 22:01:06 -0700 Subject: [PATCH 10/16] C++ initial --- glass/src/libnt/native/cpp/NTCanvas2D.cpp | 1 - myRobot/src/main/native/cpp/MyRobot.cpp | 27 ++++++++- .../src/main/native/cpp/glass/Canvas2d.cpp | 45 ++++++++++++++ .../main/native/cpp/glass/CanvasLine2d.cpp | 45 ++++++++++++++ .../main/native/include/frc/glass/Canvas2d.h | 58 +++++++++++++++++++ .../native/include/frc/glass/CanvasLine2d.h | 53 +++++++++++++++++ .../wpi/first/wpilibj/glass/CanvasText2d.java | 7 ++- 7 files changed, 232 insertions(+), 4 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/glass/Canvas2d.cpp create mode 100644 wpilibc/src/main/native/cpp/glass/CanvasLine2d.cpp create mode 100644 wpilibc/src/main/native/include/frc/glass/Canvas2d.h create mode 100644 wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp index 25b5c4421d6..faea29046b8 100644 --- a/glass/src/libnt/native/cpp/NTCanvas2D.cpp +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -217,7 +217,6 @@ void NTCanvas2DModel::Update() { m_texts.emplace_back(position, fontSize, wrapWidth, std::move(textStr), color, zOrder); - m_texts.size(); } } } diff --git a/myRobot/src/main/native/cpp/MyRobot.cpp b/myRobot/src/main/native/cpp/MyRobot.cpp index f339b95a845..ee7bb73182d 100644 --- a/myRobot/src/main/native/cpp/MyRobot.cpp +++ b/myRobot/src/main/native/cpp/MyRobot.cpp @@ -4,12 +4,37 @@ #include +#include + +#include +#include +#include + +#include "frc/geometry/Rotation2d.h" +#include "frc/glass/Canvas2d.h" +#include "frc/smartdashboard/SmartDashboard.h" +#include "frc/util/Color.h" + class MyRobot : public frc::TimedRobot { + frc::Canvas2d m_canvas{150, 150}; + frc::Rotation2d m_rotation{units::degree_t(45)}; + nt::StructArrayPublisher structTopic = + nt::NetworkTableInstance::GetDefault() + .GetStructArrayTopic("rot/asdf") + .Publish(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ - void RobotInit() override {} + void RobotInit() override { + frc::SmartDashboard::PutData("Canvas", &m_canvas); + + m_canvas.DrawLine(0, 0, 50, 50, 1, frc::Color::kRed, 255); + m_canvas.FinishFrame(); + + std::vector rot{m_rotation}; + structTopic.Set(rot); + } /** * This function is run once each time the robot enters autonomous mode diff --git a/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp b/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp new file mode 100644 index 00000000000..9893d92abe9 --- /dev/null +++ b/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp @@ -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. + +#include "frc/glass/Canvas2d.h" + +#include + +#include + +using namespace frc; + +static constexpr std::string_view kDims = "dims"; +static constexpr std::string_view kLines = "lines"; + +void Canvas2d::Clear() { + m_currentZOrder = 0; + m_lines.clear(); +} + +void Canvas2d::DrawLine(float x1, float y1, float x2, float y2, float weight, + const frc::Color8Bit& color, int opacity) { + m_lines.emplace_back(x1, y1, x2, y2, weight, color, opacity, + m_currentZOrder++); +} + +void Canvas2d::FinishFrame(bool clearCanvas) { + if (m_linesPub) { + m_linesPub.Set(m_lines); + } + + if (clearCanvas) { + Clear(); + } +} + +void Canvas2d::InitSendable(nt::NTSendableBuilder& builder) { + builder.SetSmartDashboardType("Canvas2d"); + + std::shared_ptr table = builder.GetTable(); + m_dimsPub = table->GetFloatArrayTopic(kDims).Publish(); + m_dimsPub.Set({{m_width, m_height}}); + + m_linesPub = table->GetStructArrayTopic(kLines).Publish(); +} diff --git a/wpilibc/src/main/native/cpp/glass/CanvasLine2d.cpp b/wpilibc/src/main/native/cpp/glass/CanvasLine2d.cpp new file mode 100644 index 00000000000..607852c0422 --- /dev/null +++ b/wpilibc/src/main/native/cpp/glass/CanvasLine2d.cpp @@ -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. + +#include "frc/glass/CanvasLine2d.h" + +namespace { +constexpr size_t kX1Off = 0; +constexpr size_t kY1Off = kX1Off + 4; +constexpr size_t kX2Off = kY1Off + 4; +constexpr size_t kY2Off = kX2Off + 4; +constexpr size_t kWeightOff = kY2Off + 4; +constexpr size_t kColorOff = kWeightOff + 4; +constexpr size_t kZOrderOff = kColorOff + 4; +} // namespace + +using StructType = wpi::Struct; + +frc::CanvasLine2d StructType::Unpack(std::span data) { + return frc::CanvasLine2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + frc::Color8Bit{wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, const frc::CanvasLine2d& value) { + wpi::PackStruct(data, value.x1); + wpi::PackStruct(data, value.y1); + wpi::PackStruct(data, value.x2); + wpi::PackStruct(data, value.y2); + wpi::PackStruct(data, value.weight); + wpi::PackStruct(data, static_cast(value.color.red)); + wpi::PackStruct(data, static_cast(value.color.green)); + wpi::PackStruct(data, static_cast(value.color.blue)); + wpi::PackStruct(data, static_cast(value.opacity)); + wpi::PackStruct(data, value.zOrder); +} diff --git a/wpilibc/src/main/native/include/frc/glass/Canvas2d.h b/wpilibc/src/main/native/include/frc/glass/Canvas2d.h new file mode 100644 index 00000000000..5de7cbe8ae3 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/glass/Canvas2d.h @@ -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. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include "CanvasLine2d.h" +#include "frc/util/Color8Bit.h" + +namespace frc { + +class Canvas2d : public nt::NTSendable, public wpi::SendableHelper { + public: + Canvas2d(float width, float height) : m_width{width}, m_height{height} {} + + void Clear(); + + void DrawLine(float x1, float y1, float x2, float y2, float weight, + const frc::Color8Bit& color, int opacity = 255); + + void DrawRect(float x1, float y1, float x2, float y2, float weight, bool fill, + const frc::Color8Bit& color, int opacity = 255); + + void DrawQuad(float x1, float y1, float x2, float y2, float x3, float y3, + float x4, float y4, float weight, bool fill, + const frc::Color8Bit& color, int opacity = 255); + + void DrawCircle(float x, float y, float radius, float weight, bool fill, + const frc::Color8Bit& color, int opacity = 255); + + void DrawNgon(float x, float y, float radius, int numSides, float weight, + bool fill, const frc::Color8Bit& color, int opacity = 255); + + void FinishFrame(bool clearCanvas = true); + + void InitSendable(nt::NTSendableBuilder& builder) override; + + private: + int m_currentZOrder = 0; + + const float m_width; + const float m_height; + std::vector m_lines; + + nt::FloatArrayPublisher m_dimsPub; + nt::StructArrayPublisher m_linesPub; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h new file mode 100644 index 00000000000..b649151183f --- /dev/null +++ b/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h @@ -0,0 +1,53 @@ +// 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/util/Color8Bit.h" + +namespace frc { + +struct CanvasLine2d { + float x1; + float y1; + float x2; + float y2; + float weight; + frc::Color8Bit color; + int opacity; + int zOrder; + + CanvasLine2d(float x1, float y1, float x2, float y2, float weight, + const frc::Color8Bit& color, int opacity, int zOrder) + : x1{x1}, + y1{y1}, + x2{x2}, + y2{y2}, + weight{weight}, + color{color}, + opacity{std::clamp(opacity, 0, 255)}, + zOrder{zOrder} {} +}; +} // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:CanvasLine2d"; + } + static constexpr size_t GetSize() { return 28; } + static constexpr std::string_view GetSchema() { + return "float x1;float y1;float x2;float y2;float weight;uint8 " + "color[4];int32 zOrder"; + } + + static frc::CanvasLine2d Unpack(std::span data); + static void Pack(std::span data, const frc::CanvasLine2d& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java index 6d52a849a7c..46272ebac6d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java @@ -64,12 +64,15 @@ public CanvasText2d( Color8Bit color, int opacity, int zOrder) { - // TODO: check text length + // TODO: Warn on long text? m_x = x; m_y = y; m_fontSize = size; m_wrapWidth = wrapWidth; - m_text = text; + m_text = new char[64]; + for (int i = 0; i < Math.min(text.length, m_text.length); i++) { + m_text[i] = text[i]; + } m_color = color; m_opacity = MathUtil.clamp(opacity, 0, 255); m_zOrder = zOrder; From 534f0fd60b3d6c31b62e9bc9e926aa165e8514a9 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 22 Dec 2023 11:31:30 -0800 Subject: [PATCH 11/16] Add missing includes --- glass/src/lib/native/include/glass/other/Canvas2D.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index bce93960d24..4a5330d5446 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -5,7 +5,9 @@ #pragma once #include +#include #include +#include #include #define IMGUI_DEFINE_MATH_OPERATORS From b17b7e65504daa778aec8039d7fb533a6e0094a6 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 25 Dec 2023 17:27:15 -0700 Subject: [PATCH 12/16] All in C++, move wrapWidth, default to empty array still missing texture and some cleanup --- .../lib/native/include/glass/other/Canvas2D.h | 14 ++-- glass/src/libnt/native/cpp/NTCanvas2D.cpp | 10 +-- myRobot/src/main/native/cpp/MyRobot.cpp | 19 ++--- .../src/main/native/cpp/glass/Canvas2d.cpp | 66 +++++++++++++++++ .../main/native/cpp/glass/CanvasCircle2d.cpp | 46 ++++++++++++ .../main/native/cpp/glass/CanvasNgon2d.cpp | 48 ++++++++++++ .../main/native/cpp/glass/CanvasQuad2d.cpp | 60 +++++++++++++++ .../main/native/cpp/glass/CanvasText2d.cpp | 47 ++++++++++++ .../main/native/include/frc/glass/Canvas2d.h | 18 ++++- .../native/include/frc/glass/CanvasCircle2d.h | 57 +++++++++++++++ .../native/include/frc/glass/CanvasLine2d.h | 4 + .../native/include/frc/glass/CanvasNgon2d.h | 59 +++++++++++++++ .../native/include/frc/glass/CanvasQuad2d.h | 68 +++++++++++++++++ .../native/include/frc/glass/CanvasText2d.h | 73 +++++++++++++++++++ .../edu/wpi/first/wpilibj/glass/Canvas2d.java | 27 +++---- .../first/wpilibj/glass/CanvasCircle2d.java | 3 + .../wpilibj/glass/CanvasComplexObject2d.java | 9 --- .../wpi/first/wpilibj/glass/CanvasLine2d.java | 3 + .../wpi/first/wpilibj/glass/CanvasNgon2d.java | 3 + .../wpi/first/wpilibj/glass/CanvasQuad2d.java | 3 + .../wpi/first/wpilibj/glass/CanvasText2d.java | 25 ++++--- .../main/native/include/wpi/struct/Struct.h | 13 ++++ 22 files changed, 613 insertions(+), 62 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/glass/CanvasCircle2d.cpp create mode 100644 wpilibc/src/main/native/cpp/glass/CanvasNgon2d.cpp create mode 100644 wpilibc/src/main/native/cpp/glass/CanvasQuad2d.cpp create mode 100644 wpilibc/src/main/native/cpp/glass/CanvasText2d.cpp create mode 100644 wpilibc/src/main/native/include/frc/glass/CanvasCircle2d.h create mode 100644 wpilibc/src/main/native/include/frc/glass/CanvasNgon2d.h create mode 100644 wpilibc/src/main/native/include/frc/glass/CanvasQuad2d.h create mode 100644 wpilibc/src/main/native/include/frc/glass/CanvasText2d.h delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java diff --git a/glass/src/lib/native/include/glass/other/Canvas2D.h b/glass/src/lib/native/include/glass/other/Canvas2D.h index 4a5330d5446..f848ba78211 100644 --- a/glass/src/lib/native/include/glass/other/Canvas2D.h +++ b/glass/src/lib/native/include/glass/other/Canvas2D.h @@ -160,20 +160,20 @@ class Canvas2DNgon : public Canvas2DElement { class Canvas2DText : public Canvas2DElement { public: - Canvas2DText(ImVec2 position, float fontSize, float wrapWidth, - std::string text, ImU32 color, int zOrder) + Canvas2DText(ImVec2 position, float fontSize, std::string text, ImU32 color, + float wrapWidth, int zOrder) : m_position{position}, m_fontSize{fontSize}, - m_wrapWidth{wrapWidth}, m_text(std::move(text)), m_color{color}, + m_wrapWidth{wrapWidth}, m_zOrder{zOrder} {} ImVec2 GetPosition() const { return m_position; } - float GetSize() const { return m_fontSize; } - float GetWrapWidth() const { return m_wrapWidth; } - ImU32 GetColor() const { return m_color; } + float GetFontSize() const { return m_fontSize; } std::string_view GetText() const { return m_text; } + ImU32 GetColor() const { return m_color; } + float GetWrapWidth() const { return m_wrapWidth; } int GetZOrder() const override { return m_zOrder; } void Draw(float scale, const ImVec2& cursorPos, @@ -182,9 +182,9 @@ class Canvas2DText : public Canvas2DElement { private: ImVec2 m_position; float m_fontSize; - float m_wrapWidth; std::string m_text; ImU32 m_color; + float m_wrapWidth; int m_zOrder; }; diff --git a/glass/src/libnt/native/cpp/NTCanvas2D.cpp b/glass/src/libnt/native/cpp/NTCanvas2D.cpp index faea29046b8..236ddbf9f33 100644 --- a/glass/src/libnt/native/cpp/NTCanvas2D.cpp +++ b/glass/src/libnt/native/cpp/NTCanvas2D.cpp @@ -196,9 +196,9 @@ void NTCanvas2DModel::Update() { auto* xField = textDesc->FindFieldByName("x"); auto* yField = textDesc->FindFieldByName("y"); auto* fontSizeField = textDesc->FindFieldByName("fontSize"); - auto* wrapWidthField = textDesc->FindFieldByName("wrapWidth"); auto* textField = textDesc->FindFieldByName("text"); auto* colorField = textDesc->FindFieldByName("color"); + auto* wrapWidthField = textDesc->FindFieldByName("wrapWidth"); auto* zOrderField = textDesc->FindFieldByName("zOrder"); for (auto&& v : m_textsSub.ReadQueue()) { @@ -208,15 +208,15 @@ void NTCanvas2DModel::Update() { for (const auto& text : textArray) { ImVec2 position{text.GetFloatField(xField), text.GetFloatField(yField)}; float fontSize = text.GetFloatField(fontSizeField); - float wrapWidth = text.GetFloatField(wrapWidthField); + std::string textStr{text.GetStringField(textField)}; ImU32 color = IM_COL32( text.GetUintField(colorField, 0), text.GetUintField(colorField, 1), text.GetUintField(colorField, 2), text.GetUintField(colorField, 3)); - std::string textStr{text.GetStringField(textField)}; + float wrapWidth = text.GetFloatField(wrapWidthField); int zOrder = text.GetIntField(zOrderField); - m_texts.emplace_back(position, fontSize, wrapWidth, std::move(textStr), - color, zOrder); + m_texts.emplace_back(position, fontSize, std::move(textStr), color, + wrapWidth, zOrder); } } } diff --git a/myRobot/src/main/native/cpp/MyRobot.cpp b/myRobot/src/main/native/cpp/MyRobot.cpp index ee7bb73182d..6e794eb5e96 100644 --- a/myRobot/src/main/native/cpp/MyRobot.cpp +++ b/myRobot/src/main/native/cpp/MyRobot.cpp @@ -18,22 +18,12 @@ class MyRobot : public frc::TimedRobot { frc::Canvas2d m_canvas{150, 150}; frc::Rotation2d m_rotation{units::degree_t(45)}; - nt::StructArrayPublisher structTopic = - nt::NetworkTableInstance::GetDefault() - .GetStructArrayTopic("rot/asdf") - .Publish(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ void RobotInit() override { frc::SmartDashboard::PutData("Canvas", &m_canvas); - - m_canvas.DrawLine(0, 0, 50, 50, 1, frc::Color::kRed, 255); - m_canvas.FinishFrame(); - - std::vector rot{m_rotation}; - structTopic.Set(rot); } /** @@ -64,7 +54,14 @@ class MyRobot : public frc::TimedRobot { /** * This function is called periodically during all modes */ - void RobotPeriodic() override {} + void RobotPeriodic() override { + m_canvas.DrawLine(0, 0, 50, 50, 1, frc::Color::kRed, 255); + m_canvas.DrawCircle(1, 2, 4, 5, false, {255, 255, 255}); + m_canvas.DrawNgon(25, 25, 9, 5, 6, false, {255, 255, 255}); + m_canvas.DrawQuad(25, 25, 25, 50, 50, 50, 50, 25, 3, true, {100, 0, 200}); + m_canvas.DrawText(20, 20, 10, "Hello world", {100, 200, 10}, 100); + m_canvas.FinishFrame(); + } }; int main() { diff --git a/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp b/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp index 9893d92abe9..7f4173ec7a7 100644 --- a/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp +++ b/wpilibc/src/main/native/cpp/glass/Canvas2d.cpp @@ -12,10 +12,18 @@ using namespace frc; static constexpr std::string_view kDims = "dims"; static constexpr std::string_view kLines = "lines"; +static constexpr std::string_view kQuads = "quads"; +static constexpr std::string_view kCircles = "circles"; +static constexpr std::string_view kNgons = "ngons"; +static constexpr std::string_view kTexts = "texts"; void Canvas2d::Clear() { m_currentZOrder = 0; m_lines.clear(); + m_quads.clear(); + m_circles.clear(); + m_ngons.clear(); + m_texts.clear(); } void Canvas2d::DrawLine(float x1, float y1, float x2, float y2, float weight, @@ -24,10 +32,55 @@ void Canvas2d::DrawLine(float x1, float y1, float x2, float y2, float weight, m_currentZOrder++); } +void Canvas2d::DrawRect(float x1, float y1, float x2, float y2, float weight, + bool fill, const frc::Color8Bit& color, int opacity) { + DrawQuad(x1, y1, x2, y1, x2, y2, x1, y2, weight, fill, color, opacity); +} + +void Canvas2d::DrawQuad(float x1, float y1, float x2, float y2, float x3, + float y3, float x4, float y4, float weight, bool fill, + const frc::Color8Bit& color, int opacity) { + m_quads.emplace_back(x1, y1, x2, y2, x3, y3, x4, y4, weight, fill, color, + opacity, m_currentZOrder++); +} + +void Canvas2d::DrawCircle(float x, float y, float radius, float weight, + bool fill, const frc::Color8Bit& color, int opacity) { + m_circles.emplace_back(x, y, radius, weight, fill, color, opacity, + m_currentZOrder++); +} + +void Canvas2d::DrawNgon(float x, float y, float radius, int numSides, + float weight, bool fill, const frc::Color8Bit& color, + int opacity) { + m_ngons.emplace_back(x, y, radius, numSides, weight, fill, color, opacity, + m_currentZOrder++); +} + +void Canvas2d::DrawText(float x, float y, float fontSize, + const std::string_view& text, + const frc::Color8Bit& color, int opacity, + float wrapWidth) { + m_texts.emplace_back(x, y, fontSize, text, color, opacity, wrapWidth, + m_currentZOrder++); +} + void Canvas2d::FinishFrame(bool clearCanvas) { if (m_linesPub) { m_linesPub.Set(m_lines); } + if (m_quadsPub) { + m_quadsPub.Set(m_quads); + } + if (m_circlesPub) { + m_circlesPub.Set(m_circles); + } + if (m_ngonsPub) { + m_ngonsPub.Set(m_ngons); + } + if (m_textsPub) { + m_textsPub.Set(m_texts); + } if (clearCanvas) { Clear(); @@ -42,4 +95,17 @@ void Canvas2d::InitSendable(nt::NTSendableBuilder& builder) { m_dimsPub.Set({{m_width, m_height}}); m_linesPub = table->GetStructArrayTopic(kLines).Publish(); + m_linesPub.Set({}); + + m_quadsPub = table->GetStructArrayTopic(kQuads).Publish(); + m_quadsPub.Set({}); + + m_circlesPub = table->GetStructArrayTopic(kCircles).Publish(); + m_circlesPub.Set({}); + + m_ngonsPub = table->GetStructArrayTopic(kNgons).Publish(); + m_ngonsPub.Set({}); + + m_textsPub = table->GetStructArrayTopic(kTexts).Publish(); + m_textsPub.Set({}); } diff --git a/wpilibc/src/main/native/cpp/glass/CanvasCircle2d.cpp b/wpilibc/src/main/native/cpp/glass/CanvasCircle2d.cpp new file mode 100644 index 00000000000..414bd19e3e5 --- /dev/null +++ b/wpilibc/src/main/native/cpp/glass/CanvasCircle2d.cpp @@ -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. + +#include "frc/glass/CanvasCircle2d.h" + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 4; +constexpr size_t kRadiusOff = kYOff + 4; +constexpr size_t kWeightOff = kRadiusOff + 4; +constexpr size_t kFillOff = kWeightOff + 4; +constexpr size_t kColorOff = kFillOff + 1; +constexpr size_t kZOrderOff = kColorOff + 4; +} // namespace + +using StructType = wpi::Struct; + +frc::CanvasCircle2d StructType::Unpack(std::span data) { + return frc::CanvasCircle2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + frc::Color8Bit{wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::CanvasCircle2d& value) { + wpi::PackStruct(data, value.x); + wpi::PackStruct(data, value.y); + wpi::PackStruct(data, value.radius); + wpi::PackStruct(data, value.weight); + wpi::PackStruct(data, value.fill); + wpi::PackStruct(data, static_cast(value.color.red)); + wpi::PackStruct(data, static_cast(value.color.green)); + wpi::PackStruct(data, static_cast(value.color.blue)); + wpi::PackStruct(data, static_cast(value.opacity)); + wpi::PackStruct(data, value.zOrder); +} diff --git a/wpilibc/src/main/native/cpp/glass/CanvasNgon2d.cpp b/wpilibc/src/main/native/cpp/glass/CanvasNgon2d.cpp new file mode 100644 index 00000000000..4dfb3a159e8 --- /dev/null +++ b/wpilibc/src/main/native/cpp/glass/CanvasNgon2d.cpp @@ -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. + +#include "frc/glass/CanvasNgon2d.h" + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 4; +constexpr size_t kRadiusOff = kYOff + 4; +constexpr size_t kNumSidesOff = kRadiusOff + 4; +constexpr size_t kWeightOff = kNumSidesOff + 4; +constexpr size_t kFillOff = kWeightOff + 4; +constexpr size_t kColorOff = kFillOff + 1; +constexpr size_t kZOrderOff = kColorOff + 4; +} // namespace + +using StructType = wpi::Struct; + +frc::CanvasNgon2d StructType::Unpack(std::span data) { + return frc::CanvasNgon2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + frc::Color8Bit{wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, const frc::CanvasNgon2d& value) { + wpi::PackStruct(data, value.x); + wpi::PackStruct(data, value.y); + wpi::PackStruct(data, value.radius); + wpi::PackStruct(data, value.numSides); + wpi::PackStruct(data, value.weight); + wpi::PackStruct(data, value.fill); + wpi::PackStruct(data, static_cast(value.color.red)); + wpi::PackStruct(data, static_cast(value.color.green)); + wpi::PackStruct(data, static_cast(value.color.blue)); + wpi::PackStruct(data, static_cast(value.opacity)); + wpi::PackStruct(data, value.zOrder); +} diff --git a/wpilibc/src/main/native/cpp/glass/CanvasQuad2d.cpp b/wpilibc/src/main/native/cpp/glass/CanvasQuad2d.cpp new file mode 100644 index 00000000000..5da994a9c5c --- /dev/null +++ b/wpilibc/src/main/native/cpp/glass/CanvasQuad2d.cpp @@ -0,0 +1,60 @@ +// 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/glass/CanvasQuad2d.h" + +namespace { +constexpr size_t kX1Off = 0; +constexpr size_t kY1Off = kX1Off + 4; +constexpr size_t kX2Off = kY1Off + 4; +constexpr size_t kY2Off = kX2Off + 4; +constexpr size_t kX3Off = kY2Off + 4; +constexpr size_t kY3Off = kX3Off + 4; +constexpr size_t kX4Off = kY3Off + 4; +constexpr size_t kY4Off = kX4Off + 4; +constexpr size_t kWeightOff = kY4Off + 4; +constexpr size_t kFillOff = kWeightOff + 4; +constexpr size_t kColorOff = kFillOff + 1; +constexpr size_t kZOrderOff = kColorOff + 4; +} // namespace + +using StructType = wpi::Struct; + +frc::CanvasQuad2d StructType::Unpack(std::span data) { + return frc::CanvasQuad2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + frc::Color8Bit{wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, const frc::CanvasQuad2d& value) { + wpi::PackStruct(data, value.x1); + wpi::PackStruct(data, value.y1); + wpi::PackStruct(data, value.x2); + wpi::PackStruct(data, value.y2); + wpi::PackStruct(data, value.x3); + wpi::PackStruct(data, value.y3); + wpi::PackStruct(data, value.x4); + wpi::PackStruct(data, value.y4); + wpi::PackStruct(data, value.weight); + wpi::PackStruct(data, value.fill); + wpi::PackStruct(data, static_cast(value.color.red)); + wpi::PackStruct(data, static_cast(value.color.green)); + wpi::PackStruct(data, static_cast(value.color.blue)); + wpi::PackStruct(data, static_cast(value.opacity)); + wpi::PackStruct(data, value.zOrder); +} diff --git a/wpilibc/src/main/native/cpp/glass/CanvasText2d.cpp b/wpilibc/src/main/native/cpp/glass/CanvasText2d.cpp new file mode 100644 index 00000000000..446058a36ad --- /dev/null +++ b/wpilibc/src/main/native/cpp/glass/CanvasText2d.cpp @@ -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. + +#include "frc/glass/CanvasText2d.h" + +#include + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 4; +constexpr size_t kFontSizeOff = kYOff + 4; +constexpr size_t kTextOff = kFontSizeOff + 4; +constexpr size_t kColorOff = kTextOff + 64; +constexpr size_t kWrapWidthOff = kColorOff + 4; +constexpr size_t kZOrderOff = kWrapWidthOff + 4; +} // namespace + +using StructType = wpi::Struct; + +frc::CanvasText2d StructType::Unpack(std::span data) { + return frc::CanvasText2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct, kTextOff>(data), + frc::Color8Bit{wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, const frc::CanvasText2d& value) { + wpi::PackStruct(data, value.x1); + wpi::PackStruct(data, value.y1); + wpi::PackStruct(data, value.fontSize); + wpi::PackStruct(data, value.text); + wpi::PackStruct(data, static_cast(value.color.red)); + wpi::PackStruct(data, static_cast(value.color.green)); + wpi::PackStruct(data, static_cast(value.color.blue)); + wpi::PackStruct(data, static_cast(value.opacity)); + wpi::PackStruct(data, value.wrapWidth); + wpi::PackStruct(data, value.zOrder); +} diff --git a/wpilibc/src/main/native/include/frc/glass/Canvas2d.h b/wpilibc/src/main/native/include/frc/glass/Canvas2d.h index 5de7cbe8ae3..0b2d57efad4 100644 --- a/wpilibc/src/main/native/include/frc/glass/Canvas2d.h +++ b/wpilibc/src/main/native/include/frc/glass/Canvas2d.h @@ -13,7 +13,11 @@ #include #include -#include "CanvasLine2d.h" +#include "frc/glass/CanvasCircle2d.h" +#include "frc/glass/CanvasLine2d.h" +#include "frc/glass/CanvasNgon2d.h" +#include "frc/glass/CanvasQuad2d.h" +#include "frc/glass/CanvasText2d.h" #include "frc/util/Color8Bit.h" namespace frc { @@ -40,6 +44,10 @@ class Canvas2d : public nt::NTSendable, public wpi::SendableHelper { void DrawNgon(float x, float y, float radius, int numSides, float weight, bool fill, const frc::Color8Bit& color, int opacity = 255); + void DrawText(float x, float y, float fontSize, const std::string_view& text, + const frc::Color8Bit& color, int opacity = 255, + float wrapWidth = 0); + void FinishFrame(bool clearCanvas = true); void InitSendable(nt::NTSendableBuilder& builder) override; @@ -50,9 +58,17 @@ class Canvas2d : public nt::NTSendable, public wpi::SendableHelper { const float m_width; const float m_height; std::vector m_lines; + std::vector m_quads; + std::vector m_circles; + std::vector m_ngons; + std::vector m_texts; nt::FloatArrayPublisher m_dimsPub; nt::StructArrayPublisher m_linesPub; + nt::StructArrayPublisher m_quadsPub; + nt::StructArrayPublisher m_circlesPub; + nt::StructArrayPublisher m_ngonsPub; + nt::StructArrayPublisher m_textsPub; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasCircle2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasCircle2d.h new file mode 100644 index 00000000000..7cb3c374257 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/glass/CanvasCircle2d.h @@ -0,0 +1,57 @@ +// 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/util/Color8Bit.h" + +namespace frc { + +/** + * Represents the data for a circle element on a Canvas2d. To draw on a + * Canvas2d, use Canvas2d#DrawCircle. + */ +struct CanvasCircle2d { + float x; + float y; + float radius; + float weight; + bool fill; + frc::Color8Bit color; + int opacity; + int zOrder; + + CanvasCircle2d(float x, float y, float radius, float weight, bool fill, + const frc::Color8Bit& color, int opacity, int zOrder) + : x{x}, + y{y}, + radius{radius}, + weight{weight}, + fill{fill}, + color{color}, + opacity{std::clamp(opacity, 0, 255)}, + zOrder{zOrder} {} +}; +} // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:CanvasCircle2d"; + } + static constexpr size_t GetSize() { return 25; } + static constexpr std::string_view GetSchema() { + return "float x;float y;float radius;float weight;bool fill;uint8 " + "color[4];int32 zOrder"; + } + + static frc::CanvasCircle2d Unpack(std::span data); + static void Pack(std::span data, const frc::CanvasCircle2d& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h index b649151183f..d8d5b03650b 100644 --- a/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h +++ b/wpilibc/src/main/native/include/frc/glass/CanvasLine2d.h @@ -12,6 +12,10 @@ namespace frc { +/** + * Represents the data for a line element on a Canvas2d. To draw on a Canvas2d, + * use Canvas2d#DrawLine. + */ struct CanvasLine2d { float x1; float y1; diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasNgon2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasNgon2d.h new file mode 100644 index 00000000000..c0af8720a89 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/glass/CanvasNgon2d.h @@ -0,0 +1,59 @@ +// 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/util/Color8Bit.h" + +namespace frc { + +/** + * Represents the data for a ngon element on a Canvas2d. To draw on a Canvas2d, + * use Canvas2d#DrawNgon. + */ +struct CanvasNgon2d { + float x; + float y; + float radius; + int numSides; + float weight; + bool fill; + frc::Color8Bit color; + int opacity; + int zOrder; + + CanvasNgon2d(float x, float y, float radius, int numSides, float weight, + bool fill, const frc::Color8Bit& color, int opacity, int zOrder) + : x{x}, + y{y}, + radius{radius}, + numSides{numSides}, + weight{weight}, + fill{fill}, + color{color}, + opacity{std::clamp(opacity, 0, 255)}, + zOrder{zOrder} {} +}; +} // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:CanvasNgon2d"; + } + static constexpr size_t GetSize() { return 29; } + static constexpr std::string_view GetSchema() { + return "float x;float y;float radius;int32 numSides;float weight;bool " + "fill;uint8 color[4];int32 zOrder"; + } + + static frc::CanvasNgon2d Unpack(std::span data); + static void Pack(std::span data, const frc::CanvasNgon2d& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasQuad2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasQuad2d.h new file mode 100644 index 00000000000..eac6ca1abb9 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/glass/CanvasQuad2d.h @@ -0,0 +1,68 @@ +// 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/util/Color8Bit.h" + +namespace frc { + +/** + * Represents a quad element on a Canvas2d. To draw on a Canvas2d, use + * Canvas2d#DrawQuad} or Canvas2d#DrawRect. + */ +struct CanvasQuad2d { + float x1; + float y1; + float x2; + float y2; + float x3; + float y3; + float x4; + float y4; + float weight; + bool fill; + frc::Color8Bit color; + int opacity; + int zOrder; + + CanvasQuad2d(float x1, float y1, float x2, float y2, float x3, float y3, + float x4, float y4, float weight, bool fill, + const frc::Color8Bit& color, int opacity, int zOrder) + : x1{x1}, + y1{y1}, + x2{x2}, + y2{y2}, + x3{x3}, + y3{y3}, + x4{x4}, + y4{y4}, + weight{weight}, + fill{fill}, + color{color}, + opacity{std::clamp(opacity, 0, 255)}, + zOrder{zOrder} {} +}; +} // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:CanvasQuad2d"; + } + static constexpr size_t GetSize() { return 45; } + static constexpr std::string_view GetSchema() { + return "float x1;float y1;float x2;float y2;float x3;float y3;float " + "x4;float y4;float weight;bool fill;uint8 color[4];int32 zOrder"; + } + + static frc::CanvasQuad2d Unpack(std::span data); + static void Pack(std::span data, const frc::CanvasQuad2d& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h new file mode 100644 index 00000000000..a222d69928c --- /dev/null +++ b/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h @@ -0,0 +1,73 @@ +// 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 + +#include "frc/util/Color8Bit.h" + +namespace frc { + +/** + * Represents the data for a text element on a Canvas2d. To draw on a Canvas2d, + * use Canvas2d#DrawText. + */ +struct CanvasText2d { + float x1; + float y1; + float fontSize; + std::array text; + frc::Color8Bit color; + int opacity; + float wrapWidth; + int zOrder; + + CanvasText2d(float x1, float y1, float fontSize, std::string_view text, + const frc::Color8Bit& color, int opacity, float wrapWidth, + int zOrder) + : x1{x1}, + y1{y1}, + fontSize{fontSize}, + color{color}, + opacity{std::clamp(opacity, 0, 255)}, + wrapWidth{wrapWidth}, + zOrder{zOrder} { + std::copy_n(text.begin(), std::min(text.size(), this->text.size()), + this->text.begin()); + } + + CanvasText2d(float x1, float y1, float fontSize, std::array text, + const frc::Color8Bit& color, int opacity, float wrapWidth, + int zOrder) + : x1{x1}, + y1{y1}, + fontSize{fontSize}, + text{text}, + color{color}, + opacity{std::clamp(opacity, 0, 255)}, + wrapWidth{wrapWidth}, + zOrder{zOrder} {} +}; +} // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:CanvasText2d"; + } + static constexpr size_t GetSize() { return 88; } + static constexpr std::string_view GetSchema() { + return "float x;float y;float fontSize;char text[64];uint8 color[4];float " + "wrapWidth;int32 zOrder"; + } + + static frc::CanvasText2d Unpack(std::span data); + static void Pack(std::span data, const frc::CanvasText2d& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index c3e49db5bab..318adb46bf2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -43,15 +43,6 @@ public Canvas2d(float width, float length) { m_dims[1] = length; } - /** - * Draw a complex object on the canvas. - * - * @param object The object - */ - public void draw(CanvasComplexObject2d object) { - object.drawOn(this); - } - /** Clears the canvas. */ public void clear() { m_currentZOrder = 0; @@ -99,9 +90,7 @@ public void drawRect( boolean fill, Color8Bit color, int opacity) { - m_quads.add( - new CanvasQuad2d( - x1, y1, x2, y1, x2, y2, x1, y2, weight, fill, color, opacity, m_currentZOrder++)); + drawQuad(x1, y1, x2, y1, x2, y2, x1, y2, weight, fill, color, opacity); } /** @@ -186,21 +175,20 @@ public void drawNgon( * @param x The x coordinate of the text * @param y The y coordinate of the text * @param fontSize The size of the text - * @param wrapWidth The width at which the text should wrap. 0 for no wrap * @param text The text. The max length of the text is 64 characters * @param color The color of the text * @param opacity The opacity of the text [0-255] + * @param wrapWidth The width at which the text should wrap. 0 for no wrap */ public void drawText( float x, float y, float fontSize, - float wrapWidth, String text, Color8Bit color, - int opacity) { + int opacity, float wrapWidth) { m_texts.add( - new CanvasText2d(x, y, fontSize, wrapWidth, text, color, opacity, m_currentZOrder++)); + new CanvasText2d(x, y, fontSize, text, color, opacity, wrapWidth, m_currentZOrder++)); } /** Finish and push the frame to Sendable. Clears the canvas after pushing the frame. */ @@ -209,7 +197,7 @@ public void finishFrame() { } /** - * Finish and push the frame to Sendable. + * Finish and push the frame to Sendable. If sendable is not initialized, this does nothing. * * @param clearCanvas Whether to clear the canvas after pushing the frame. */ @@ -245,26 +233,31 @@ public void initSendable(NTSendableBuilder builder) { m_linesPub.close(); } m_linesPub = table.getStructArrayTopic("lines", CanvasLine2d.struct).publish(); + m_linesPub.set(new CanvasLine2d[0]); if (m_quadsPub != null) { m_quadsPub.close(); } m_quadsPub = table.getStructArrayTopic("quads", CanvasQuad2d.struct).publish(); + m_quadsPub.set(new CanvasQuad2d[0]); if (m_circlesPub != null) { m_circlesPub.close(); } m_circlesPub = table.getStructArrayTopic("circles", CanvasCircle2d.struct).publish(); + m_circlesPub.set(new CanvasCircle2d[0]); if (m_ngonsPub != null) { m_ngonsPub.close(); } m_ngonsPub = table.getStructArrayTopic("ngons", CanvasNgon2d.struct).publish(); + m_ngonsPub.set(new CanvasNgon2d[0]); if (m_textsPub != null) { m_textsPub.close(); } m_textsPub = table.getStructArrayTopic("texts", CanvasText2d.struct).publish(); + m_textsPub.set(new CanvasText2d[0]); } @Override diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java index 58423d3aac1..19b877e45f3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import java.nio.ByteBuffer; +/** + * Represents the data for a circle element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawCircle}. + */ public class CanvasCircle2d { public final float m_x; public final float m_y; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java deleted file mode 100644 index c7fa21b4ce8..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasComplexObject2d.java +++ /dev/null @@ -1,9 +0,0 @@ -// 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.wpilibj.glass; - -public abstract class CanvasComplexObject2d { - public abstract void drawOn(Canvas2d canvas); -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java index a58d52347fa..3cc316de3f3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import java.nio.ByteBuffer; +/** + * Represents the data for a line element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawLine}. + */ public class CanvasLine2d { public final float m_x1; public final float m_y1; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java index 0ebfe8170ca..7f180b9cf65 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import java.nio.ByteBuffer; +/** + * Represents the data for a ngon element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawNgon}. + */ public class CanvasNgon2d { public final float m_x; public final float m_y; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java index 93cb24c166c..8c7f72b75c6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import java.nio.ByteBuffer; +/** + * Represents a quad element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawQuad} or {@link Canvas2d#drawRect}. + */ public class CanvasQuad2d { public final float m_x1; public final float m_y1; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java index 46272ebac6d..94420158588 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java @@ -9,12 +9,15 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import java.nio.ByteBuffer; +/** + * Represents the data for a text element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawText}. + */ class CanvasText2d { public final float m_x; public final float m_y; public final float m_fontSize; - public final float m_wrapWidth; public final char[] m_text; + public final float m_wrapWidth; public final Color8Bit m_color; public final int m_opacity; public final int m_zOrder; @@ -35,12 +38,12 @@ public CanvasText2d( float x, float y, float fontSize, - float wrapWidth, String text, Color8Bit color, int opacity, + float wrapWidth, int zOrder) { - this(x, y, fontSize, wrapWidth, text.toCharArray(), color, opacity, zOrder); + this(x, y, fontSize, text.toCharArray(), color, opacity, wrapWidth, zOrder); } /** @@ -59,22 +62,22 @@ public CanvasText2d( float x, float y, float size, - float wrapWidth, char[] text, Color8Bit color, int opacity, + float wrapWidth, int zOrder) { // TODO: Warn on long text? m_x = x; m_y = y; m_fontSize = size; - m_wrapWidth = wrapWidth; m_text = new char[64]; for (int i = 0; i < Math.min(text.length, m_text.length); i++) { m_text[i] = text[i]; } m_color = color; m_opacity = MathUtil.clamp(opacity, 0, 255); + m_wrapWidth = wrapWidth; m_zOrder = zOrder; } @@ -91,12 +94,12 @@ public String getTypeString() { @Override public int getSize() { - return kSizeFloat * 4 + kSizeInt8 * 64 + kSizeInt8 * 4 + kSizeInt32; + return kSizeFloat * 3 + kSizeInt8 * 64 + kSizeFloat + kSizeInt8 * 4 + kSizeInt32; } @Override public String getSchema() { - return "float x;float y;float fontSize;float wrapWidth;char text[64];uint8 color[4];int32 zOrder"; + return "float x;float y;float fontSize;char text[64];uint8 color[4];float wrapWidth;int32 zOrder"; } @Override @@ -104,25 +107,23 @@ public CanvasText2d unpack(ByteBuffer bb) { float x = bb.getFloat(); float y = bb.getFloat(); float fontSize = bb.getFloat(); - float wrapWidth = bb.getFloat(); char[] text = new char[64]; for (int i = 0; i < 64; i++) { text[i] = (char) bb.get(); } Color8Bit color = new Color8Bit(bb.get(), bb.get(), bb.get()); int opacity = bb.get(); + float wrapWidth = bb.getFloat(); int zOrder = bb.getInt(); - return new CanvasText2d(x, y, fontSize, wrapWidth, text, color, opacity, zOrder); + return new CanvasText2d(x, y, fontSize, text, color, opacity, wrapWidth, zOrder); } @Override public void pack(ByteBuffer bb, CanvasText2d value) { - System.out.println(bb.position()); bb.putFloat(value.m_x); bb.putFloat(value.m_y); bb.putFloat(value.m_fontSize); - bb.putFloat(value.m_wrapWidth); for (int i = 0; i < value.m_text.length; i++) { bb.put((byte) value.m_text[i]); } @@ -133,8 +134,8 @@ public void pack(ByteBuffer bb, CanvasText2d value) { bb.put((byte) value.m_color.green); bb.put((byte) value.m_color.blue); bb.put((byte) value.m_opacity); + bb.putFloat(value.m_wrapWidth); bb.putInt(value.m_zOrder); - System.out.println(bb.position()); } } diff --git a/wpiutil/src/main/native/include/wpi/struct/Struct.h b/wpiutil/src/main/native/include/wpi/struct/Struct.h index 58aca475d9e..116f5b0b326 100644 --- a/wpiutil/src/main/native/include/wpi/struct/Struct.h +++ b/wpiutil/src/main/native/include/wpi/struct/Struct.h @@ -392,6 +392,19 @@ struct Struct { } }; +/** + * Raw struct support for char values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view GetTypeString() { return "struct:char"; } + static constexpr size_t GetSize() { return 1; } + static constexpr std::string_view GetSchema() { return "char value"; } + static char Unpack(std::span data) { return data[0]; } + static void Pack(std::span data, char value) { data[0] = value; } +}; + /** * Raw struct support for uint8_t values. * Primarily useful for higher level struct implementations. From d16002f4bbd4ae7a5681779b55d68c2d98189241 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 25 Dec 2023 17:58:20 -0700 Subject: [PATCH 13/16] Add missing include, no idea why CI fails... --- wpilibc/src/main/native/include/frc/glass/CanvasText2d.h | 1 + 1 file changed, 1 insertion(+) diff --git a/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h b/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h index a222d69928c..b053112fe10 100644 --- a/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h +++ b/wpilibc/src/main/native/include/frc/glass/CanvasText2d.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include From bb5ff4d3c2d4defdaef082b356b1521a8d2d7071 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 25 Dec 2023 17:59:12 -0700 Subject: [PATCH 14/16] Forgot to java format --- .../src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java | 3 ++- .../main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java | 3 ++- .../main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java | 3 ++- .../main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java | 3 ++- .../main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java | 3 ++- .../main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java | 3 ++- 6 files changed, 12 insertions(+), 6 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java index 318adb46bf2..bbd6c789d36 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/Canvas2d.java @@ -186,7 +186,8 @@ public void drawText( float fontSize, String text, Color8Bit color, - int opacity, float wrapWidth) { + int opacity, + float wrapWidth) { m_texts.add( new CanvasText2d(x, y, fontSize, text, color, opacity, wrapWidth, m_currentZOrder++)); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java index 19b877e45f3..9301e07efc4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasCircle2d.java @@ -10,7 +10,8 @@ import java.nio.ByteBuffer; /** - * Represents the data for a circle element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawCircle}. + * Represents the data for a circle element on a Canvas2d. To draw on a Canvas2d, use {@link + * Canvas2d#drawCircle}. */ public class CanvasCircle2d { public final float m_x; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java index 3cc316de3f3..a3f8da3de0c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasLine2d.java @@ -10,7 +10,8 @@ import java.nio.ByteBuffer; /** - * Represents the data for a line element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawLine}. + * Represents the data for a line element on a Canvas2d. To draw on a Canvas2d, use {@link + * Canvas2d#drawLine}. */ public class CanvasLine2d { public final float m_x1; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java index 7f180b9cf65..a6448cc1d66 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasNgon2d.java @@ -10,7 +10,8 @@ import java.nio.ByteBuffer; /** - * Represents the data for a ngon element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawNgon}. + * Represents the data for a ngon element on a Canvas2d. To draw on a Canvas2d, use {@link + * Canvas2d#drawNgon}. */ public class CanvasNgon2d { public final float m_x; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java index 8c7f72b75c6..0522ee851d6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasQuad2d.java @@ -10,7 +10,8 @@ import java.nio.ByteBuffer; /** - * Represents a quad element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawQuad} or {@link Canvas2d#drawRect}. + * Represents a quad element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawQuad} or + * {@link Canvas2d#drawRect}. */ public class CanvasQuad2d { public final float m_x1; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java index 94420158588..e7c3c407ff5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/glass/CanvasText2d.java @@ -10,7 +10,8 @@ import java.nio.ByteBuffer; /** - * Represents the data for a text element on a Canvas2d. To draw on a Canvas2d, use {@link Canvas2d#drawText}. + * Represents the data for a text element on a Canvas2d. To draw on a Canvas2d, use {@link + * Canvas2d#drawText}. */ class CanvasText2d { public final float m_x; From 47d6642af09ac37558cf1a54c740afd56bc47669 Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 25 Dec 2023 18:25:09 -0700 Subject: [PATCH 15/16] Fix java myRobot --- myRobot/src/main/java/frc/robot/MyRobot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java index 0a114014718..61314d5aea0 100644 --- a/myRobot/src/main/java/frc/robot/MyRobot.java +++ b/myRobot/src/main/java/frc/robot/MyRobot.java @@ -40,7 +40,7 @@ public void robotInit() { m_canvas2D.drawRect(50, 50, 100, 100, 0, true, new Color8Bit(Color.kGreen), 255); m_canvas2D.drawCircle(105, 105, 10, 3, false, new Color8Bit(Color.kBlue), 100); m_canvas2D.drawNgon(120, 120, 10, 5, 1, false, new Color8Bit(Color.kYellow), 255); - m_canvas2D.drawText(5, 5, 5, -1, "Hello world", new Color8Bit(255, 2, 51), 255); + m_canvas2D.drawText(5, 5, 5, "Hello world", new Color8Bit(255, 2, 51), 255, 0); m_canvas2D.finishFrame(); } From e224c6178d3db1d37060096b75e26f35b84f31fb Mon Sep 17 00:00:00 2001 From: ohowe Date: Mon, 25 Dec 2023 18:30:49 -0700 Subject: [PATCH 16/16] so thats there now cause I merged but how did CI get it??? so confused --- glass/src/libnt/native/cpp/StandardNetworkTables.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp index ed879d5aa93..98dbe333236 100644 --- a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp +++ b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp @@ -171,7 +171,8 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { }); provider.Register( NTProfiledPIDControllerModel::kType, - [](nt::NetworkTableInstance inst, const char* path) { + [](nt::NetworkTableInstance inst, + wpi::StructDescriptorDatabase& structDatabase, const char* path) { return std::make_unique(inst, path); }, [](Window* win, Model* model, const char* path) {