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

[glass] Field2d enhancements (#3234)

Browse Source 
- Add raw support for pose lists > 255/3 in length
- Improve drag selection, especially with closely overlapping objects
- Drag selection of corner also highlights center of object with smaller circle
- Multiple styles (box, line, closed line, track)
- Configurable line and arrow settings (color, weight)
- Add tooltip for object name, index, x, y, rotation
- Context menu for pose edit/add/remove
- View/edit in feet or inches as well as meters
- Configurable object selectability

Implementation: use vector of Pose2d internally, use units
This commit is contained in:
Peter Johnson
2021-03-27 13:34:44 -07:00
committed by GitHub
parent ffb590bfcc
commit c97acd18e7
7 changed files with 1121 additions and 438 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
View File

@@ -4,6 +4,11 @@
#include "frc/smartdashboard/FieldObject2d.h"
#include <vector>
#include <wpi/Endian.h>
#include <wpi/MathExtras.h>
using namespace frc;
FieldObject2d::FieldObject2d(FieldObject2d&& rhs) {
@@ -66,17 +71,41 @@ void FieldObject2d::UpdateEntry(bool setDefault) {
if (!m_entry) {
return;
}
wpi::SmallVector<double, 9> arr;
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
arr.push_back(translation.X().to<double>());
arr.push_back(translation.Y().to<double>());
arr.push_back(pose.Rotation().Degrees().to<double>());
}
if (setDefault) {
m_entry.SetDefaultDoubleArray(arr);
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().to<double>());
arr.push_back(translation.Y().to<double>());
arr.push_back(pose.Rotation().Degrees().to<double>());
}
if (setDefault) {
m_entry.SetDefaultDoubleArray(arr);
} else {
m_entry.ForceSetDoubleArray(arr);
}
} else {
m_entry.SetDoubleArray(arr);
// 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().to<double>()));
p += 8;
wpi::support::endian::write64be(
p, wpi::DoubleToBits(translation.Y().to<double>()));
p += 8;
wpi::support::endian::write64be(
p, wpi::DoubleToBits(pose.Rotation().Degrees().to<double>()));
p += 8;
}
if (setDefault) {
m_entry.SetDefaultRaw(wpi::StringRef{arr.data(), arr.size()});
} else {
m_entry.ForceSetRaw(wpi::StringRef{arr.data(), arr.size()});
}
}
}
@@ -85,18 +114,45 @@ void FieldObject2d::UpdateFromEntry() const {
return;
}
auto val = m_entry.GetValue();
if (!val || !val->IsDoubleArray()) {
if (!val) {
return;
}
auto arr = val->GetDoubleArray();
auto size = arr.size();
if ((size % 3) != 0) {
return;
}
m_poses.resize(size / 3);
for (size_t i = 0; i < size / 3; ++i) {
m_poses[i] =
Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
if (val->IsDoubleArray()) {
auto arr = val->GetDoubleArray();
auto size = arr.size();
if ((size % 3) != 0) {
return;
}
m_poses.resize(size / 3);
for (size_t i = 0; i < size / 3; ++i) {
m_poses[i] =
Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
}
} else if (val->IsRaw()) {
// treat it simply as an array of doubles
wpi::StringRef data = val->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.begin();
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] = Pose2d{units::meter_t{x}, units::meter_t{y},
Rotation2d{units::degree_t{rot}}};
}
}
}