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

Simulation GUI: Add 2D field view (#2261)

Browse Source 
The field image and robot image can be loaded or just a wireframe used.
The robot can be moved and rotated with a mouse click + drag.
The robot position is settable in robot code via the Field2d class.
This commit is contained in:
Peter Johnson
2020-02-01 21:30:23 -08:00
committed by GitHub
parent 42da07396c
commit c165dc5e50
16 changed files with 2325 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.styleguide
View File

@@ -11,6 +11,7 @@ cppSrcFileInclude {
generatedFileExclude {
FRCNetComm\.java$
simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
simulation/halsim_gui/src/main/native/include/portable-file-dialogs\.h$
}
repoRootNameOverride {

1
ThirdPartyNotices.txt
View File

@@ -44,6 +44,7 @@ Team 254 Library wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineP
wpilibc/src/main/native/include/spline/SplineParameterizer.h
wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h
wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
Portable File Dialogs simulation/halsim_gui/src/main/native/include/portable-file-dialogs.h
==============================================================================

9
imgui/CMakeLists.txt
View File

@@ -32,6 +32,11 @@ file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.h
set_source_files_properties(${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp
PROPERTIES OBJECT_DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/ProggyDotted.inc)
# stb_image
file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp
CONTENT "#define STBI_WINDOWS_UTF8\n#define STB_IMAGE_IMPLEMENTATION\n#include \"stb_image.h\"\n"
)
# Add imgui directly to our build.
set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
set(BUILD_SHARED_LIBS OFF)
@@ -46,8 +51,8 @@ add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/gl3w-src
set(imgui_srcdir ${CMAKE_CURRENT_BINARY_DIR}/imgui-src)
file(GLOB imgui_sources ${imgui_srcdir}/*.cpp)
add_library(imgui STATIC ${imgui_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp)
add_library(imgui STATIC ${imgui_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp)
target_link_libraries(imgui PUBLIC gl3w glfw)
target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/stb-src>")
set_property(TARGET imgui PROPERTY POSITION_INDEPENDENT_CODE ON)

10
imgui/CMakeLists.txt.in
View File

@@ -41,3 +41,13 @@ ExternalProject_Add(proggyfonts
INSTALL_COMMAND ""
TEST_COMMAND ""
)
ExternalProject_Add(stb
GIT_REPOSITORY https://github.com/nothings/stb.git
GIT_TAG f67165c2bb2af3060ecae7d20d6f731173485ad0
SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/stb-src"
BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/stb-build"
CONFIGURE_COMMAND ""
BUILD_COMMAND ""
INSTALL_COMMAND ""
TEST_COMMAND ""
)

2
shared/config.gradle
View File

@@ -11,7 +11,7 @@ nativeUtils {
niLibVersion = "2020.10.1"
opencvVersion = "3.4.7-2"
googleTestVersion = "1.9.0-4-437e100-1"
imguiVersion = "1.72b-2"
imguiVersion = "1.72b-3"
}
}
}

645
simulation/halsim_gui/src/main/native/cpp/Field2D.cpp Normal file
View File

@@ -0,0 +1,645 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Field2D.h"
#include <cmath>
#include <GL/gl3w.h>
#include <hal/SimDevice.h>
#include <imgui.h>
#define IMGUI_DEFINE_MATH_OPERATORS
#include <imgui_internal.h>
#include <mockdata/SimDeviceData.h>
#include <units/units.h>
#include <wpi/Path.h>
#include <wpi/SmallString.h>
#include <wpi/json.h>
#include <wpi/raw_istream.h>
#include <wpi/raw_ostream.h>
#include "GuiUtil.h"
#include "HALSimGui.h"
#include "SimDeviceGui.h"
#include "portable-file-dialogs.h"
using namespace halsimgui;
namespace {
// Per-frame field data (not persistent)
struct FieldFrameData {
// in window coordinates
ImVec2 imageMin;
ImVec2 imageMax;
ImVec2 min;
ImVec2 max;
float scale; // scaling from field units to screen units
};
class FieldInfo {
public:
static constexpr float kDefaultWidth = 15.98f;
static constexpr float kDefaultHeight = 8.21f;
std::unique_ptr<pfd::open_file> m_fileOpener;
float m_width = kDefaultWidth;
float m_height = kDefaultHeight;
void Reset();
void LoadImage();
void LoadJson(const wpi::Twine& jsonfile);
FieldFrameData GetFrameData() const;
void Draw(ImDrawList* drawList, const ImVec2& windowPos,
const FieldFrameData& frameData) const;
bool ReadIni(wpi::StringRef name, wpi::StringRef value);
void WriteIni(ImGuiTextBuffer* out) const;
private:
bool LoadImageImpl(const wpi::Twine& fn);
std::string m_filename;
GLuint m_texture = 0;
int m_imageWidth = 0;
int m_imageHeight = 0;
int m_top = 0;
int m_left = 0;
int m_bottom = -1;
int m_right = -1;
};
// Per-frame robot data (not persistent)
struct RobotFrameData {
// in window coordinates
ImVec2 center;
ImVec2 corners[4];
ImVec2 arrow[3];
// scaled width/2 and length/2, in screen units
float width2;
float length2;
};
class RobotInfo {
public:
static constexpr float kDefaultWidth = 0.6858f;
static constexpr float kDefaultLength = 0.8204f;
std::unique_ptr<pfd::open_file> m_fileOpener;
float m_width = kDefaultWidth;
float m_length = kDefaultLength;
void Reset();
void LoadImage();
void UpdateFromSimDevice();
void SetPosition(double x, double y);
// set and get rotation in radians
void SetRotation(double rot);
double GetRotation() const {
return units::convert<units::degrees, units::radians>(m_rot);
}
RobotFrameData GetFrameData(const FieldFrameData& ffd) const;
void Draw(ImDrawList* drawList, const ImVec2& windowPos,
const RobotFrameData& frameData, int hit, float hitRadius) const;
bool ReadIni(wpi::StringRef name, wpi::StringRef value);
void WriteIni(ImGuiTextBuffer* out) const;
private:
bool LoadImageImpl(const wpi::Twine& fn);
std::string m_filename;
GLuint m_texture = 0;
HAL_SimDeviceHandle m_devHandle = 0;
hal::SimDouble m_xHandle;
hal::SimDouble m_yHandle;
hal::SimDouble m_rotHandle;
double m_x = 0;
double m_y = 0;
double m_rot = 0;
};
} // namespace
static FieldInfo gField;
static RobotInfo gRobot;
static int gDragRobot = 0;
static ImVec2 gDragInitialOffset;
static double gDragInitialAngle;
// read/write settings to ini file
static void* Field2DReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
const char* name) {
if (name == wpi::StringRef{"Field"}) return &gField;
if (name == wpi::StringRef{"Robot"}) return &gRobot;
return nullptr;
}
static void Field2DReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
void* entry, const char* lineStr) {
wpi::StringRef line{lineStr};
auto [name, value] = line.split('=');
name = name.trim();
value = value.trim();
if (entry == &gField)
gField.ReadIni(name, value);
else if (entry == &gRobot)
gRobot.ReadIni(name, value);
}
static void Field2DWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
ImGuiTextBuffer* out_buf) {
gField.WriteIni(out_buf);
gRobot.WriteIni(out_buf);
}
void FieldInfo::Reset() {
if (m_texture != 0) glDeleteTextures(1, &m_texture);
m_texture = 0;
m_filename.clear();
m_imageWidth = 0;
m_imageHeight = 0;
m_top = 0;
m_left = 0;
m_bottom = -1;
m_right = -1;
}
void FieldInfo::LoadImage() {
if (m_fileOpener && m_fileOpener->ready(0)) {
if (wpi::StringRef(m_fileOpener->result()[0]).endswith(".json")) {
LoadJson(m_fileOpener->result()[0]);
} else {
LoadImageImpl(m_fileOpener->result()[0]);
m_top = 0;
m_left = 0;
m_bottom = -1;
m_right = -1;
}
m_fileOpener.reset();
}
if (m_texture == 0 && !m_filename.empty()) {
if (!LoadImageImpl(m_filename)) m_filename.clear();
}
}
void FieldInfo::LoadJson(const wpi::Twine& jsonfile) {
std::error_code ec;
wpi::raw_fd_istream f(jsonfile, ec);
if (ec) {
wpi::errs() << "GUI: could not open field JSON file\n";
return;
}
// parse file
wpi::json j;
try {
j = wpi::json::parse(f);
} catch (const wpi::json::parse_error& e) {
wpi::errs() << "GUI: JSON: could not parse: " << e.what() << '\n';
}
// top level must be an object
if (!j.is_object()) {
wpi::errs() << "GUI: JSON: does not contain a top object\n";
return;
}
// image filename
std::string image;
try {
image = j.at("field-image").get<std::string>();
} catch (const wpi::json::exception& e) {
wpi::errs() << "GUI: JSON: could not read field-image: " << e.what()
<< '\n';
return;
}
// corners
int top, left, bottom, right;
try {
top = j.at("field-corners").at("top-left").at(1).get<int>();
left = j.at("field-corners").at("top-left").at(0).get<int>();
bottom = j.at("field-corners").at("bottom-right").at(1).get<int>();
right = j.at("field-corners").at("bottom-right").at(0).get<int>();
} catch (const wpi::json::exception& e) {
wpi::errs() << "GUI: JSON: could not read field-corners: " << e.what()
<< '\n';
return;
}
// size
float width;
float height;
try {
width = j.at("field-size").at(0).get<float>();
height = j.at("field-size").at(1).get<float>();
} catch (const wpi::json::exception& e) {
wpi::errs() << "GUI: JSON: could not read field-size: " << e.what() << '\n';
return;
}
// units for size
std::string unit;
try {
unit = j.at("field-unit").get<std::string>();
} catch (const wpi::json::exception& e) {
wpi::errs() << "GUI: JSON: could not read field-unit: " << e.what() << '\n';
return;
}
// convert size units to meters
if (unit == "foot" || unit == "feet") {
width = units::convert<units::feet, units::meters>(width);
height = units::convert<units::feet, units::meters>(height);
}
// the image filename is relative to the json file
wpi::SmallString<128> pathname;
jsonfile.toVector(pathname);
wpi::sys::path::remove_filename(pathname);
wpi::sys::path::append(pathname, image);
// load field image
if (!LoadImageImpl(pathname)) return;
// save to field info
m_filename = pathname.str();
m_top = top;
m_left = left;
m_bottom = bottom;
m_right = right;
m_width = width;
m_height = height;
}
bool FieldInfo::LoadImageImpl(const wpi::Twine& fn) {
wpi::outs() << "GUI: loading field image '" << fn << "'\n";
GLuint oldTexture = m_texture;
if (!LoadTextureFromFile(fn, &m_texture, &m_imageWidth, &m_imageHeight)) {
wpi::errs() << "GUI: could not read field image\n";
return false;
}
if (oldTexture != 0) glDeleteTextures(1, &oldTexture);
m_filename = fn.str();
return true;
}
FieldFrameData FieldInfo::GetFrameData() const {
FieldFrameData ffd;
// get window content region
ffd.imageMin = ImGui::GetWindowContentRegionMin();
ffd.imageMax = ImGui::GetWindowContentRegionMax();
// fit the image into the window
if (m_texture != 0 && m_imageHeight != 0 && m_imageWidth != 0)
MaxFit(&ffd.imageMin, &ffd.imageMax, m_imageWidth, m_imageHeight);
ImVec2 min = ffd.imageMin;
ImVec2 max = ffd.imageMax;
// size down the box by the image corners (if any)
if (m_bottom > 0 && m_right > 0) {
min.x += m_left * (max.x - min.x) / m_imageWidth;
min.y += m_top * (max.y - min.y) / m_imageHeight;
max.x -= (m_imageWidth - m_right) * (max.x - min.x) / m_imageWidth;
max.y -= (m_imageHeight - m_bottom) * (max.y - min.y) / m_imageHeight;
}
// draw the field "active area" as a yellow boundary box
MaxFit(&min, &max, m_width, m_height);
ffd.min = min;
ffd.max = max;
ffd.scale = (max.x - min.x) / m_width;
return ffd;
}
void FieldInfo::Draw(ImDrawList* drawList, const ImVec2& windowPos,
const FieldFrameData& ffd) const {
if (m_texture != 0 && m_imageHeight != 0 && m_imageWidth != 0) {
drawList->AddImage(
reinterpret_cast<ImTextureID>(static_cast<uintptr_t>(m_texture)),
windowPos + ffd.imageMin, windowPos + ffd.imageMax);
}
// draw the field "active area" as a yellow boundary box
drawList->AddRect(windowPos + ffd.min, windowPos + ffd.max,
IM_COL32(255, 255, 0, 255));
}
bool FieldInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
if (name == "image") {
m_filename = value;
} else if (name == "top") {
int num;
if (value.getAsInteger(10, num)) return true;
m_top = num;
} else if (name == "left") {
int num;
if (value.getAsInteger(10, num)) return true;
m_left = num;
} else if (name == "bottom") {
int num;
if (value.getAsInteger(10, num)) return true;
m_bottom = num;
} else if (name == "right") {
int num;
if (value.getAsInteger(10, num)) return true;
m_right = num;
} else if (name == "width") {
std::sscanf(value.data(), "%f", &m_width);
} else if (name == "height") {
std::sscanf(value.data(), "%f", &m_height);
} else {
return false;
}
return true;
}
void FieldInfo::WriteIni(ImGuiTextBuffer* out) const {
out->appendf(
"[Field2D][Field]\nimage=%s\ntop=%d\nleft=%d\nbottom=%d\nright=%d\nwidth="
"%f\nheight=%f\n\n",
m_filename.c_str(), m_top, m_left, m_bottom, m_right, m_width, m_height);
}
void RobotInfo::Reset() {
if (m_texture != 0) glDeleteTextures(1, &m_texture);
m_texture = 0;
m_filename.clear();
}
void RobotInfo::LoadImage() {
if (m_fileOpener && m_fileOpener->ready(0)) {
LoadImageImpl(m_fileOpener->result()[0]);
m_fileOpener.reset();
}
if (m_texture == 0 && !m_filename.empty()) {
if (!LoadImageImpl(m_filename)) m_filename.clear();
}
}
bool RobotInfo::LoadImageImpl(const wpi::Twine& fn) {
wpi::outs() << "GUI: loading robot image '" << fn << "'\n";
GLuint oldTexture = m_texture;
if (!LoadTextureFromFile(fn, &m_texture, nullptr, nullptr)) {
wpi::errs() << "GUI: could not read robot image\n";
return false;
}
if (oldTexture != 0) glDeleteTextures(1, &oldTexture);
m_filename = fn.str();
return true;
}
void RobotInfo::UpdateFromSimDevice() {
if (m_devHandle == 0) m_devHandle = HALSIM_GetSimDeviceHandle("Field2D");
if (m_devHandle == 0) return;
if (!m_xHandle) m_xHandle = HALSIM_GetSimValueHandle(m_devHandle, "x");
if (m_xHandle) m_x = m_xHandle.Get();
if (!m_yHandle) m_yHandle = HALSIM_GetSimValueHandle(m_devHandle, "y");
if (m_yHandle) m_y = m_yHandle.Get();
if (!m_rotHandle) m_rotHandle = HALSIM_GetSimValueHandle(m_devHandle, "rot");
if (m_rotHandle) m_rot = m_rotHandle.Get();
}
void RobotInfo::SetPosition(double x, double y) {
m_x = x;
m_y = y;
if (m_xHandle) m_xHandle.Set(x);
if (m_yHandle) m_yHandle.Set(y);
}
void RobotInfo::SetRotation(double rot) {
double rotDegrees = units::convert<units::radians, units::degrees>(rot);
// force to -180 to +180 range
rotDegrees = rotDegrees + std::ceil((-rotDegrees - 180) / 360) * 360;
m_rot = rotDegrees;
if (m_rotHandle) m_rotHandle.Set(rotDegrees);
}
RobotFrameData RobotInfo::GetFrameData(const FieldFrameData& ffd) const {
RobotFrameData rfd;
float width2 = ffd.scale * m_width / 2;
float length2 = ffd.scale * m_length / 2;
// (0,0) origin is bottom left
ImVec2 center(ffd.min.x + ffd.scale * m_x, ffd.max.y - ffd.scale * m_y);
// build rotated points around center
double rot = GetRotation();
float cos_a = std::cos(-rot);
float sin_a = std::sin(-rot);
rfd.corners[0] = center + ImRotate(ImVec2(-length2, -width2), cos_a, sin_a);
rfd.corners[1] = center + ImRotate(ImVec2(length2, -width2), cos_a, sin_a);
rfd.corners[2] = center + ImRotate(ImVec2(length2, width2), cos_a, sin_a);
rfd.corners[3] = center + ImRotate(ImVec2(-length2, width2), cos_a, sin_a);
rfd.arrow[0] =
center + ImRotate(ImVec2(-length2 / 2, -width2 / 2), cos_a, sin_a);
rfd.arrow[1] = center + ImRotate(ImVec2(length2 / 2, 0), cos_a, sin_a);
rfd.arrow[2] =
center + ImRotate(ImVec2(-length2 / 2, width2 / 2), cos_a, sin_a);
rfd.center = center;
rfd.width2 = width2;
rfd.length2 = length2;
return rfd;
}
void RobotInfo::Draw(ImDrawList* drawList, const ImVec2& windowPos,
const RobotFrameData& rfd, int hit,
float hitRadius) const {
if (m_texture != 0) {
drawList->AddImageQuad(
reinterpret_cast<ImTextureID>(static_cast<uintptr_t>(m_texture)),
windowPos + rfd.corners[0], windowPos + rfd.corners[1],
windowPos + rfd.corners[2], windowPos + rfd.corners[3]);
} else {
drawList->AddQuad(windowPos + rfd.corners[0], windowPos + rfd.corners[1],
windowPos + rfd.corners[2], windowPos + rfd.corners[3],
IM_COL32(255, 0, 0, 255), 4.0);
drawList->AddTriangle(windowPos + rfd.arrow[0], windowPos + rfd.arrow[1],
windowPos + rfd.arrow[2], IM_COL32(0, 255, 0, 255),
4.0);
}
if (hit > 0) {
if (hit == 1) {
drawList->AddCircle(windowPos + rfd.center, hitRadius,
IM_COL32(0, 255, 0, 255));
} else {
drawList->AddCircle(windowPos + rfd.corners[hit - 2], hitRadius,
IM_COL32(0, 255, 0, 255));
}
}
}
bool RobotInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
if (name == "image") {
m_filename = value;
} else if (name == "width") {
std::sscanf(value.data(), "%f", &m_width);
} else if (name == "length") {
std::sscanf(value.data(), "%f", &m_length);
} else {
return false;
}
return true;
}
void RobotInfo::WriteIni(ImGuiTextBuffer* out) const {
out->appendf("[Field2D][Robot]\nimage=%s\nwidth=%f\nlength=%f\n\n",
m_filename.c_str(), m_width, m_length);
}
static void OptionMenuField2D() {
if (ImGui::BeginMenu("2D Field View")) {
if (ImGui::MenuItem("Choose field image...")) {
gField.m_fileOpener = std::make_unique<pfd::open_file>(
"Choose field image", "",
std::vector<std::string>{"Image File",
"*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
"*.hdr *.pic *.ppm *.pgm",
"PathWeaver JSON File", "*.json"});
}
if (ImGui::MenuItem("Reset field image")) {
gField.Reset();
}
if (ImGui::MenuItem("Choose robot image...")) {
gRobot.m_fileOpener = std::make_unique<pfd::open_file>(
"Choose robot image", "",
std::vector<std::string>{"Image File",
"*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
"*.hdr *.pic *.ppm *.pgm"});
}
if (ImGui::MenuItem("Reset robot image")) {
gRobot.Reset();
}
ImGui::EndMenu();
}
}
static void DisplayField2DSettings() {
ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
ImGui::InputFloat("Field Width", &gField.m_width);
ImGui::InputFloat("Field Height", &gField.m_height);
// ImGui::InputInt("Field Top", &gField.m_top);
// ImGui::InputInt("Field Left", &gField.m_left);
// ImGui::InputInt("Field Right", &gField.m_right);
// ImGui::InputInt("Field Bottom", &gField.m_bottom);
ImGui::InputFloat("Robot Width", &gRobot.m_width);
ImGui::InputFloat("Robot Length", &gRobot.m_length);
ImGui::PopItemWidth();
}
static void DisplayField2D() {
// load images
gField.LoadImage();
gRobot.LoadImage();
// get robot coordinates from SimDevice
gRobot.UpdateFromSimDevice();
FieldFrameData ffd = gField.GetFrameData();
RobotFrameData rfd = gRobot.GetFrameData(ffd);
ImVec2 windowPos = ImGui::GetWindowPos();
// for dragging to work, there needs to be a button (otherwise the window is
// dragged)
ImGui::InvisibleButton("field", ImGui::GetContentRegionAvail());
// allow dragging the robot around
ImVec2 cursor = ImGui::GetIO().MousePos - windowPos;
int hit = 0;
float hitRadius = (std::min)(rfd.width2, rfd.length2) / 2;
// only allow initiation of dragging when invisible button is hovered; this
// prevents the window resize handles from simultaneously activating the drag
// functionality
if (ImGui::IsItemHovered()) {
float hitRadiusSquared = hitRadius * hitRadius;
// it's within the hit radius of the center?
if (GetDistSquared(cursor, rfd.center) < hitRadiusSquared)
hit = 1;
else if (GetDistSquared(cursor, rfd.corners[0]) < hitRadiusSquared)
hit = 2;
else if (GetDistSquared(cursor, rfd.corners[1]) < hitRadiusSquared)
hit = 3;
else if (GetDistSquared(cursor, rfd.corners[2]) < hitRadiusSquared)
hit = 4;
else if (GetDistSquared(cursor, rfd.corners[3]) < hitRadiusSquared)
hit = 5;
if (hit > 0 && ImGui::IsMouseClicked(0)) {
if (hit == 1) {
gDragRobot = hit;
gDragInitialOffset = cursor - rfd.center;
} else {
gDragRobot = hit;
ImVec2 off = cursor - rfd.center;
gDragInitialAngle = std::atan2(off.y, off.x) + gRobot.GetRotation();
}
}
}
if (gDragRobot > 0 && ImGui::IsMouseDown(0)) {
if (gDragRobot == 1) {
ImVec2 newPos = cursor - gDragInitialOffset;
gRobot.SetPosition(
(std::clamp(newPos.x, ffd.min.x, ffd.max.x) - ffd.min.x) / ffd.scale,
(ffd.max.y - std::clamp(newPos.y, ffd.min.y, ffd.max.y)) / ffd.scale);
rfd = gRobot.GetFrameData(ffd);
} else {
ImVec2 off = cursor - rfd.center;
gRobot.SetRotation(gDragInitialAngle - std::atan2(off.y, off.x));
}
hit = gDragRobot; // keep it highlighted
} else {
gDragRobot = 0;
}
// draw
auto drawList = ImGui::GetWindowDrawList();
gField.Draw(drawList, windowPos, ffd);
gRobot.Draw(drawList, windowPos, rfd, hit, hitRadius);
}
void Field2D::Initialize() {
// hook ini handler to save settings
ImGuiSettingsHandler iniHandler;
iniHandler.TypeName = "Field2D";
iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
iniHandler.ReadOpenFn = Field2DReadOpen;
iniHandler.ReadLineFn = Field2DReadLine;
iniHandler.WriteAllFn = Field2DWriteAll;
ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
HALSimGui::AddOptionMenu(OptionMenuField2D);
HALSimGui::AddWindow("2D Field Settings", DisplayField2DSettings,
ImGuiWindowFlags_AlwaysAutoResize);
HALSimGui::SetWindowVisibility("2D Field Settings", HALSimGui::kHide);
HALSimGui::SetDefaultWindowPos("2D Field Settings", 200, 150);
HALSimGui::AddWindow("2D Field View", DisplayField2D);
HALSimGui::SetWindowVisibility("2D Field View", HALSimGui::kHide);
HALSimGui::SetDefaultWindowPos("2D Field View", 200, 200);
HALSimGui::SetDefaultWindowSize("2D Field View", 400, 200);
HALSimGui::SetWindowPadding("2D Field View", 0, 0);
// SimDeviceGui::Hide("Field2D");
}

17
simulation/halsim_gui/src/main/native/cpp/Field2D.h Normal file
View File

@@ -0,0 +1,17 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
namespace halsimgui {
class Field2D {
public:
static void Initialize();
};
} // namespace halsimgui

62
simulation/halsim_gui/src/main/native/cpp/GuiUtil.cpp Normal file
View File

@@ -0,0 +1,62 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "GuiUtil.h"
#include <stb_image.h>
#include <wpi/SmallString.h>
bool halsimgui::LoadTextureFromFile(const wpi::Twine& filename,
GLuint* out_texture, int* out_width,
int* out_height) {
wpi::SmallString<128> buf;
// Load from file
int width = 0;
int height = 0;
unsigned char* data =
stbi_load(filename.toNullTerminatedStringRef(buf).data(), &width, &height,
nullptr, 4);
if (!data) return false;
// Create a OpenGL texture identifier
GLuint texture;
glGenTextures(1, &texture);
glBindTexture(GL_TEXTURE_2D, texture);
// Setup filtering parameters for display
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
// Upload pixels into texture
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA,
GL_UNSIGNED_BYTE, data);
stbi_image_free(data);
*out_texture = texture;
if (out_width) *out_width = width;
if (out_height) *out_height = height;
return true;
}
void halsimgui::MaxFit(ImVec2* min, ImVec2* max, float width, float height) {
float destWidth = max->x - min->x;
float destHeight = max->y - min->y;
if (width == 0 || height == 0) return;
if (destWidth * height > destHeight * width) {
float outputWidth = width * destHeight / height;
min->x += (destWidth - outputWidth) / 2;
max->x -= (destWidth - outputWidth) / 2;
} else {
float outputHeight = height * destWidth / width;
min->y += (destHeight - outputHeight) / 2;
max->y -= (destHeight - outputHeight) / 2;
}
}

17
simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
View File

@@ -40,6 +40,8 @@ struct WindowInfo {
ImGuiCond sizeCond = 0;
ImVec2 pos;
ImVec2 size;
bool setPadding = false;
ImVec2 padding;
};
} // namespace
@@ -248,6 +250,14 @@ void HALSimGui::SetDefaultWindowSize(const char* name, float width,
window.size = ImVec2{width, height};
}
void HALSimGui::SetWindowPadding(const char* name, float x, float y) {
auto it = gWindowMap.find(name);
if (it == gWindowMap.end()) return;
auto& window = gWindows[it->second];
window.setPadding = true;
window.padding = ImVec2{x, y};
}
bool HALSimGui::AreOutputsDisabled() {
return gDisableOutputsOnDSDisable && !HALSIM_GetDriverStationEnabled();
}
@@ -521,9 +531,12 @@ void HALSimGui::Main(void*) {
ImGui::SetNextWindowPos(window.pos, window.posCond);
if (window.sizeCond != 0)
ImGui::SetNextWindowSize(window.size, window.sizeCond);
if (window.setPadding)
ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, window.padding);
if (ImGui::Begin(window.name.c_str(), &window.visible, window.flags))
window.display();
ImGui::End();
if (window.setPadding) ImGui::PopStyleVar();
}
}
@@ -597,6 +610,10 @@ void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
HALSimGui::SetDefaultWindowSize(name, width, height);
}
void HALSIMGUI_SetWindowPadding(const char* name, float x, float y) {
HALSimGui::SetDefaultWindowSize(name, x, y);
}
int HALSIMGUI_AreOutputsDisabled(void) {
return HALSimGui::AreOutputsDisabled();
}

4
simulation/halsim_gui/src/main/native/cpp/main.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,6 +17,7 @@
#include "DIOGui.h"
#include "DriverStationGui.h"
#include "EncoderGui.h"
#include "Field2D.h"
#include "HALSimGui.h"
#include "PDPGui.h"
#include "PWMGui.h"
@@ -42,6 +43,7 @@ __declspec(dllexport)
HALSimGui::Add(DriverStationGui::Initialize);
HALSimGui::Add(DIOGui::Initialize);
HALSimGui::Add(EncoderGui::Initialize);
HALSimGui::Add(Field2D::Initialize);
HALSimGui::Add(PDPGui::Initialize);
HALSimGui::Add(PWMGui::Initialize);
HALSimGui::Add(RelayGui::Initialize);

29
simulation/halsim_gui/src/main/native/include/GuiUtil.h Normal file
View File

@@ -0,0 +1,29 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <GL/gl3w.h>
#include <imgui.h>
#include <wpi/Twine.h>
namespace halsimgui {
bool LoadTextureFromFile(const wpi::Twine& filename, GLuint* out_texture,
int* out_width, int* out_height);
// get distance^2 between two ImVec's
inline float GetDistSquared(const ImVec2& a, const ImVec2& b) {
float deltaX = b.x - a.x;
float deltaY = b.y - a.y;
return deltaX * deltaX + deltaY * deltaY;
}
// maximize fit while preserving aspect ratio
void MaxFit(ImVec2* min, ImVec2* max, float width, float height);
} // namespace halsimgui

11
simulation/halsim_gui/src/main/native/include/HALSimGui.h
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -23,6 +23,7 @@ void HALSIMGUI_SetWindowVisibility(const char* name, int32_t visibility);
void HALSIMGUI_SetDefaultWindowPos(const char* name, float x, float y);
void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
float height);
void HALSIMGUI_SetWindowPadding(const char* name, float x, float y);
int HALSIMGUI_AreOutputsDisabled(void);
} // extern "C"
@@ -127,6 +128,14 @@ class HALSimGui {
*/
static void SetDefaultWindowSize(const char* name, float width, float height);
/**
* Sets internal padding of window added with AddWindow().
* @param name window name (same as name passed to AddWindow())
* @param x horizontal padding
* @param y vertical padding
*/
static void SetWindowPadding(const char* name, float x, float y);
/**
* Returns true if outputs are disabled.
*

1305
simulation/halsim_gui/src/main/native/include/portable-file-dialogs.h Normal file
View File

File diff suppressed because it is too large Load Diff

49
wpilibc/src/main/native/cpp/simulation/Field2d.cpp Normal file
View File

@@ -0,0 +1,49 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/simulation/Field2d.h"
using namespace frc;
Field2d::Field2d() : m_device{"Field2D"} {
if (m_device) {
m_x = m_device.CreateDouble("x", false, 0.0);
m_y = m_device.CreateDouble("y", false, 0.0);
m_rot = m_device.CreateDouble("rot", false, 0.0);
}
}
void Field2d::SetRobotPose(const Pose2d& pose) {
if (m_device) {
auto& translation = pose.Translation();
m_x.Set(translation.X().to<double>());
m_y.Set(translation.Y().to<double>());
m_rot.Set(pose.Rotation().Degrees().to<double>());
} else {
m_pose = pose;
}
}
void Field2d::SetRobotPose(units::meter_t x, units::meter_t y,
Rotation2d rotation) {
if (m_device) {
m_x.Set(x.to<double>());
m_y.Set(y.to<double>());
m_rot.Set(rotation.Degrees().to<double>());
} else {
m_pose = Pose2d{x, y, rotation};
}
}
Pose2d Field2d::GetRobotPose() {
if (m_device) {
return Pose2d{units::meter_t{m_x.Get()}, units::meter_t{m_y.Get()},
Rotation2d{units::degree_t{m_rot.Get()}}};
} else {
return m_pose;
}
}

70
wpilibc/src/main/native/include/frc/simulation/Field2d.h Normal file
View File

@@ -0,0 +1,70 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <hal/SimDevice.h>
#include <units/units.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
namespace frc {
/**
* 2D representation of game field (for simulation).
*
* In non-simulation mode this simply stores and returns the robot pose.
*
* The robot pose is the actual location shown on the simulation view.
* This may or may not match the robot's internal odometry. For example, if
* the robot is shown at a particular starting location, the pose in this
* class would represent the actual location on the field, but the robot's
* internal state might have a 0,0,0 pose (unless it's initialized to
* something different).
*
* As the user is able to edit the pose, code performing updates should get
* the robot pose, transform it as appropriate (e.g. based on simulated wheel
* velocity), and set the new pose.
*/
class Field2d {
public:
Field2d();
/**
* Set the robot pose from a Pose object.
*
* @param pose 2D pose
*/
void SetRobotPose(const Pose2d& pose);
/**
* Set the robot pose from x, y, and rotation.
*
* @param x X location
* @param y Y location
* @param rotation rotation
*/
void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
/**
* Get the robot pose.
*
* @return 2D pose
*/
Pose2d GetRobotPose();
private:
Pose2d m_pose;
hal::SimDevice m_device;
hal::SimDouble m_x;
hal::SimDouble m_y;
hal::SimDouble m_rot;
};
} // namespace frc

98
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java Normal file
View File

@@ -0,0 +1,98 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
/**
* 2D representation of game field (for simulation).
*
* <p>In non-simulation mode this simply stores and returns the robot pose.
*
* <p>The robot pose is the actual location shown on the simulation view.
* This may or may not match the robot's internal odometry. For example, if
* the robot is shown at a particular starting location, the pose in this
* class would represent the actual location on the field, but the robot's
* internal state might have a 0,0,0 pose (unless it's initialized to
* something different).
*
* <p>As the user is able to edit the pose, code performing updates should get
* the robot pose, transform it as appropriate (e.g. based on simulated wheel
* velocity), and set the new pose.
*/
public class Field2d {
public Field2d() {
m_device = SimDevice.create("Field2D");
if (m_device != null) {
m_x = m_device.createDouble("x", false, 0.0);
m_y = m_device.createDouble("y", false, 0.0);
m_rot = m_device.createDouble("rot", false, 0.0);
}
}
/**
* Set the robot pose from a Pose object.
*
* @param pose 2D pose
*/
public void setRobotPose(Pose2d pose) {
if (m_device != null) {
Translation2d translation = pose.getTranslation();
m_x.set(translation.getX());
m_y.set(translation.getY());
m_rot.set(pose.getRotation().getDegrees());
} else {
m_pose = pose;
}
}
/**
* Set the robot pose from x, y, and rotation.
*
* @param xMeters X location, in meters
* @param yMeters Y location, in meters
* @param rotation rotation
*/
public void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
if (m_device != null) {
m_x.set(xMeters);
m_y.set(yMeters);
m_rot.set(rotation.getDegrees());
} else {
m_pose = new Pose2d(xMeters, yMeters, rotation);
}
}
/**
* Get the robot pose.
*
* @return 2D pose
*/
public Pose2d getRobotPose() {
if (m_device != null) {
return new Pose2d(m_x.get(), m_y.get(), Rotation2d.fromDegrees(m_rot.get()));
} else {
return m_pose;
}
}
private Pose2d m_pose;
@SuppressWarnings("MemberName")
private final SimDevice m_device;
@SuppressWarnings("MemberName")
private SimDouble m_x;
@SuppressWarnings("MemberName")
private SimDouble m_y;
private SimDouble m_rot;
}