Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[ntcore] NetworkTables 4 (#3217)

Browse Source
This commit is contained in:
Peter Johnson
2022-10-08 10:01:31 -07:00
committed by GitHub
parent 90cfa00115
commit 77301b126c
380 changed files with 34573 additions and 22095 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
glass/src/libnt/native/cpp/NTField2D.cpp
View File

@@ -8,6 +8,8 @@
#include <vector>
#include <fmt/format.h>
#include <networktables/DoubleArrayTopic.h>
#include <networktables/MultiSubscriber.h>
#include <ntcore_cpp.h>
#include <wpi/Endian.h>
#include <wpi/MathExtras.h>
@@ -18,20 +20,16 @@ using namespace glass;
class NTField2DModel::ObjectModel : public FieldObjectModel {
public:
ObjectModel(std::string_view name, NT_Entry entry)
: m_name{name}, m_entry{entry} {}
ObjectModel(std::string_view name, nt::DoubleArrayTopic topic)
: m_name{name}, m_topic{topic} {}
const char* GetName() const override { return m_name.c_str(); }
NT_Entry GetEntry() const { return m_entry; }
nt::DoubleArrayTopic GetTopic() const { return m_topic; }
void NTUpdate(const nt::Value& value);
void Update() override {
if (auto value = nt::GetEntryValue(m_entry)) {
NTUpdate(*value);
}
}
bool Exists() override { return nt::GetEntryType(m_entry) != NT_UNASSIGNED; }
void Update() override {}
bool Exists() override { return m_topic.Exists(); }
bool IsReadOnly() override { return false; }
wpi::span<const frc::Pose2d> GetPoses() override { return m_poses; }
@@ -44,7 +42,8 @@ class NTField2DModel::ObjectModel : public FieldObjectModel {
void UpdateNT();
std::string m_name;
NT_Entry m_entry;
nt::DoubleArrayTopic m_topic;
nt::DoubleArrayPublisher m_pub;
std::vector<frc::Pose2d> m_poses;
};
@@ -62,63 +61,21 @@ void NTField2DModel::ObjectModel::NTUpdate(const nt::Value& value) {
units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
frc::Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
}
} else if (value.IsRaw()) {
// treat it simply as an array of doubles
std::string_view data = value.GetRaw();
// must be triples of doubles
auto size = data.size();
if ((size % (3 * 8)) != 0) {
return;
}
m_poses.resize(size / (3 * 8));
const char* p = data.data();
for (size_t i = 0; i < size / (3 * 8); ++i) {
double x = wpi::BitsToDouble(
wpi::support::endian::readNext<uint64_t, wpi::support::big,
wpi::support::unaligned>(p));
double y = wpi::BitsToDouble(
wpi::support::endian::readNext<uint64_t, wpi::support::big,
wpi::support::unaligned>(p));
double rot = wpi::BitsToDouble(
wpi::support::endian::readNext<uint64_t, wpi::support::big,
wpi::support::unaligned>(p));
m_poses[i] = frc::Pose2d{units::meter_t{x}, units::meter_t{y},
frc::Rotation2d{units::degree_t{rot}}};
}
}
}
void NTField2DModel::ObjectModel::UpdateNT() {
if (m_poses.size() < (255 / 3)) {
wpi::SmallVector<double, 9> arr;
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
arr.push_back(translation.X().value());
arr.push_back(translation.Y().value());
arr.push_back(pose.Rotation().Degrees().value());
}
nt::SetEntryTypeValue(m_entry, nt::Value::MakeDoubleArray(arr));
} else {
// send as raw array of doubles if too big for NT array
std::vector<char> arr;
arr.resize(m_poses.size() * 3 * 8);
char* p = arr.data();
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
wpi::support::endian::write64be(
p, wpi::DoubleToBits(translation.X().value()));
p += 8;
wpi::support::endian::write64be(
p, wpi::DoubleToBits(translation.Y().value()));
p += 8;
wpi::support::endian::write64be(
p, wpi::DoubleToBits(pose.Rotation().Degrees().value()));
p += 8;
}
nt::SetEntryTypeValue(m_entry,
nt::Value::MakeRaw({arr.data(), arr.size()}));
wpi::SmallVector<double, 9> arr;
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
arr.push_back(translation.X().value());
arr.push_back(translation.Y().value());
arr.push_back(pose.Rotation().Degrees().value());
}
if (!m_pub) {
m_pub = m_topic.Publish();
}
m_pub.Set(arr);
}
void NTField2DModel::ObjectModel::SetPoses(wpi::span<const frc::Pose2d> poses) {
@@ -149,69 +106,75 @@ void NTField2DModel::ObjectModel::SetRotation(size_t i, frc::Rotation2d rot) {
}
NTField2DModel::NTField2DModel(std::string_view path)
: NTField2DModel{nt::GetDefaultInstance(), path} {}
: NTField2DModel{nt::NetworkTableInstance::GetDefault(), path} {}
NTField2DModel::NTField2DModel(NT_Inst inst, std::string_view path)
: m_nt{inst},
m_path{fmt::format("{}/", path)},
m_name{m_nt.GetEntry(fmt::format("{}/.name", path))} {
m_nt.AddListener(m_path, NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_DELETE |
NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE);
NTField2DModel::NTField2DModel(nt::NetworkTableInstance inst,
std::string_view path)
: m_path{fmt::format("{}/", path)},
m_inst{inst},
m_tableSub{inst,
{{m_path}},
{{nt::PubSubOption::SendAll(true),
nt::PubSubOption::Periodic(0.05)}}},
m_nameTopic{inst.GetTopic(fmt::format("{}/.name", path))},
m_topicListener{inst},
m_valueListener{inst} {
m_topicListener.Add(m_tableSub, NT_TOPIC_NOTIFY_PUBLISH |
NT_TOPIC_NOTIFY_UNPUBLISH |
NT_TOPIC_NOTIFY_IMMEDIATE);
m_valueListener.Add(m_tableSub,
NT_VALUE_NOTIFY_IMMEDIATE | NT_VALUE_NOTIFY_LOCAL);
}
NTField2DModel::~NTField2DModel() = default;
void NTField2DModel::Update() {
for (auto&& event : m_nt.PollListener()) {
// handle publish/unpublish
for (auto&& event : m_topicListener.ReadQueue()) {
auto name = wpi::drop_front(event.info.name, m_path.size());
if (name.empty() || name[0] == '.') {
continue;
}
auto [it, match] = Find(event.info.name);
if (event.flags & NT_TOPIC_NOTIFY_UNPUBLISH) {
if (match) {
m_objects.erase(it);
}
continue;
} else if (event.flags & NT_TOPIC_NOTIFY_PUBLISH) {
if (!match) {
it = m_objects.emplace(
it, std::make_unique<ObjectModel>(
event.info.name, nt::DoubleArrayTopic{event.info.topic}));
}
} else if (!match) {
continue;
}
}
// update values
for (auto&& event : m_valueListener.ReadQueue()) {
// .name
if (event.entry == m_name) {
if (event.value && event.value->IsString()) {
m_nameValue = event.value->GetString();
if (event.topic == m_nameTopic.GetHandle()) {
if (event.value && event.value.IsString()) {
m_nameValue = event.value.GetString();
}
continue;
}
// common case: update of existing entry; search by entry
if (event.flags & NT_NOTIFY_UPDATE) {
auto it = std::find_if(
m_objects.begin(), m_objects.end(),
[&](const auto& e) { return e->GetEntry() == event.entry; });
if (it != m_objects.end()) {
(*it)->NTUpdate(*event.value);
continue;
}
}
// handle create/delete
std::string_view name = event.name;
if (wpi::starts_with(name, m_path)) {
name.remove_prefix(m_path.size());
if (name.empty() || name[0] == '.') {
continue;
}
auto [it, match] = Find(event.name);
if (event.flags & NT_NOTIFY_DELETE) {
if (match) {
m_objects.erase(it);
}
continue;
} else if (event.flags & NT_NOTIFY_NEW) {
if (!match) {
it = m_objects.emplace(
it, std::make_unique<ObjectModel>(event.name, event.entry));
}
} else if (!match) {
continue;
}
if (event.flags & (NT_NOTIFY_NEW | NT_NOTIFY_UPDATE)) {
(*it)->NTUpdate(*event.value);
}
auto it =
std::find_if(m_objects.begin(), m_objects.end(), [&](const auto& e) {
return e->GetTopic().GetHandle() == event.topic;
});
if (it != m_objects.end()) {
(*it)->NTUpdate(event.value);
continue;
}
}
}
bool NTField2DModel::Exists() {
return m_nt.IsConnected() && nt::GetEntryType(m_name) != NT_UNASSIGNED;
return m_inst.IsConnected() && m_nameTopic.Exists();
}
bool NTField2DModel::IsReadOnly() {
@@ -222,8 +185,9 @@ FieldObjectModel* NTField2DModel::AddFieldObject(std::string_view name) {
auto fullName = fmt::format("{}{}", m_path, name);
auto [it, match] = Find(fullName);
if (!match) {
it = m_objects.emplace(
it, std::make_unique<ObjectModel>(fullName, m_nt.GetEntry(fullName)));
it = m_objects.emplace(it,
std::make_unique<ObjectModel>(
fullName, m_inst.GetDoubleArrayTopic(fullName)));
}
return it->get();
}
@@ -231,7 +195,6 @@ FieldObjectModel* NTField2DModel::AddFieldObject(std::string_view name) {
void NTField2DModel::RemoveFieldObject(std::string_view name) {
auto [it, match] = Find(fmt::format("{}{}", m_path, name));
if (match) {
nt::DeleteEntry((*it)->GetEntry());
m_objects.erase(it);
}
}