diff --git a/glass/src/libcs/native/cpp/Camera.cpp b/glass/src/libcs/native/cpp/Camera.cpp index 2b7c793a26e..4970b756658 100644 --- a/glass/src/libcs/native/cpp/Camera.cpp +++ b/glass/src/libcs/native/cpp/Camera.cpp @@ -452,98 +452,3 @@ void CameraView::Display() { } void CameraView::Hidden() {} - -void glass::DisplayCameraModelTable(Storage& root, int kinds) { - if (!ImGui::BeginTable("cameras", 7, - ImGuiTableFlags_RowBg | ImGuiTableFlags_BordersInnerV | - ImGuiTableFlags_SizingFixedFit)) { - return; - } - - ImGui::TableSetupColumn("Id"); - ImGui::TableSetupColumn("Mode"); - ImGui::TableSetupColumn("Enabled"); - ImGui::TableSetupColumn("Active"); - ImGui::TableSetupColumn("FPS", ImGuiTableColumnFlags_WidthFixed, - ImGui::GetFontSize() * 5); - ImGui::TableSetupColumn("Data Rate", ImGuiTableColumnFlags_WidthFixed, - ImGui::GetFontSize() * 5); - ImGui::TableSetupColumn("Actions"); - ImGui::TableHeadersRow(); - - std::string toDelete; - for (auto&& kv : root.GetChildren()) { - CameraModel* model = kv.value().GetData(); - if (!model) { - continue; - } - if ((model->GetKind() & kinds) == 0) { - continue; - } - - ImGui::TableNextRow(); - ImGui::TableNextColumn(); - ImGui::Selectable(model->GetId().c_str()); - if (ImGui::BeginDragDropSource()) { - ImGui::SetDragDropPayload("Camera", model->GetId().data(), - model->GetId().size()); - ImGui::Text("Camera: %s", model->GetId().c_str()); - ImGui::EndDragDropSource(); - } - - ImGui::TableNextColumn(); - char buf[64]; - VideoModeToString(buf, sizeof(buf), model->GetVideoMode()); - ImGui::TextUnformatted(buf); - ImGui::SameLine(); - if (ImGui::SmallButton(">")) { - ImGui::OpenPopup("mode"); - } - if (ImGui::BeginPopup("mode")) { - if (EditMode(model)) { - ImGui::CloseCurrentPopup(); - } - ImGui::EndPopup(); - } - - ImGui::TableNextColumn(); - ImGui::TextUnformatted(model->Exists() ? "Yes" : "No"); - - ImGui::TableNextColumn(); - ImGui::TextUnformatted(model->GetSource().IsEnabled() ? "Yes" : "No"); - - ImGui::TableNextColumn(); - ImGui::Text("%2.1f FPS", model->GetActualFPS()); - - ImGui::TableNextColumn(); - wpi::format_to_n_c_str(buf, sizeof(buf), "{:.1f} Mbps", - model->GetActualDataRate() * 8 / 1000000); - ImGui::TextUnformatted(buf); - - ImGui::TableNextColumn(); - if (ImGui::SmallButton(model->GetSource().IsEnabled() ? "Stop" : "Start")) { - if (model->Exists()) { - model->Stop(); - } else { - model->Start(); - } - } - ImGui::SameLine(); - if (ImGui::SmallButton("Delete")) { - toDelete = kv.key(); - } - ImGui::SameLine(); - if (ImGui::SmallButton("Properties")) { - ImGui::OpenPopup("properties"); - } - if (ImGui::BeginPopup("properties")) { - EditProperties(model->GetSource()); - ImGui::EndPopup(); - } - } - if (!toDelete.empty()) { - root.Erase(toDelete); - } - - ImGui::EndTable(); -} diff --git a/glass/src/libcs/native/cpp/CameraProvider.cpp b/glass/src/libcs/native/cpp/CameraProvider.cpp index 8d2a9f9a568..23903e5ea66 100644 --- a/glass/src/libcs/native/cpp/CameraProvider.cpp +++ b/glass/src/libcs/native/cpp/CameraProvider.cpp @@ -4,13 +4,109 @@ #include "glass/camera/CameraProvider.h" +#include +#include + #include "glass/Storage.h" using namespace glass; + +static void DisplayCameraModelTable(Storage& root, int kinds) { + if (!ImGui::BeginTable("cameras", 4, + ImGuiTableFlags_RowBg | ImGuiTableFlags_BordersInnerV | + ImGuiTableFlags_SizingFixedFit)) { + return; + } + + ImGui::TableSetupColumn("Name"); + ImGui::TableSetupColumn("FPS", ImGuiTableColumnFlags_WidthFixed, + ImGui::GetFontSize() * 5); + ImGui::TableSetupColumn("Data Rate", ImGuiTableColumnFlags_WidthFixed, + ImGui::GetFontSize() * 5); + ImGui::TableSetupColumn("Actions"); + ImGui::TableHeadersRow(); + + std::string toDelete; + for (auto&& kv : root.GetChildren()) { + CameraModel* model = kv.value().GetData(); + if (!model) { + continue; + } + if ((model->GetKind() & kinds) == 0) { + continue; + } + + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::Selectable(model->GetId().c_str()); + if (ImGui::BeginDragDropSource()) { + ImGui::SetDragDropPayload("Camera", model->GetId().data(), + model->GetId().size()); + ImGui::Text("Camera: %s", model->GetId().c_str()); + ImGui::EndDragDropSource(); + } + + ImGui::TableNextColumn(); + char buf[64]; + VideoModeToString(buf, sizeof(buf), model->GetVideoMode()); + ImGui::TextUnformatted(buf); + ImGui::SameLine(); + if (ImGui::SmallButton(">")) { + ImGui::OpenPopup("mode"); + } + if (ImGui::BeginPopup("mode")) { + if (EditMode(model)) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + ImGui::TableNextColumn(); + ImGui::TextUnformatted(model->Exists() ? "Yes" : "No"); + + ImGui::TableNextColumn(); + ImGui::TextUnformatted(model->GetSource().IsEnabled() ? "Yes" : "No"); + + ImGui::TableNextColumn(); + ImGui::Text("%2.1f FPS", model->GetActualFPS()); + + ImGui::TableNextColumn(); + wpi::format_to_n_c_str(buf, sizeof(buf), "{:.1f} Mbps", + model->GetActualDataRate() * 8 / 1000000); + ImGui::TextUnformatted(buf); + + ImGui::TableNextColumn(); + if (ImGui::SmallButton(model->GetSource().IsEnabled() ? "Stop" : "Start")) { + if (model->Exists()) { + model->Stop(); + } else { + model->Start(); + } + } + ImGui::SameLine(); + if (ImGui::SmallButton("Delete")) { + toDelete = kv.key(); + } + ImGui::SameLine(); + if (ImGui::SmallButton("Properties")) { + ImGui::OpenPopup("properties"); + } + if (ImGui::BeginPopup("properties")) { + EditProperties(model->GetSource()); + ImGui::EndPopup(); + } + } + if (!toDelete.empty()) { + root.Erase(toDelete); + } + + ImGui::EndTable(); +} + void DisplayCameraList() { if (glass::imm::BeginWindow(gCameraListWindow)) { - glass::DisplayCameraModelTable(cameras); + DisplayCameraModelTable(cameras); if (ImGui::Button("Add USB")) { ImGui::OpenPopup("Add USB Camera"); } @@ -57,6 +153,100 @@ void DisplayCameraList() { glass::PopStorageStack(); } #endif + +CameraProvider::CameraProvider(Storage& storage) + : CameraProvider{storage, nt::NetworkTableInstance::GetDefault()} {} + +CameraProvider::CameraProvider(Storage& storage, + nt::NetworkTableInstance inst) + : WindowManager{storage}, m_inst{inst}, m_poller{inst} { + m_listener = + m_poller.AddListener({{"/CameraPublisher/"}}, + nt::EventFlags::kTopic | nt::EventFlags::kValueAll); +} + +void NTCameraProvider::Update() { + for (nt::Event& event : m_poller.ReadQueue()) { + if (nt::TopicInfo* topicInfo = event.GetTopicInfo()) { + wpi::SmallVector parts; + wpi::split(topicInfo->name, parts, '/', -1, false); + if (parts.size() < 3) { + continue; + } + + // find/create NTSourceInfo by name + std::string_view sourceName = parts[1]; + auto it = std::find_if( + m_sourceInfo.begin(), m_sourceInfo.end(), + [&](const auto& info) { return info->name == sourceName; }); + NTSourceInfo* info; + if (it == m_sourceInfo.end()) { + info = + m_sourceInfo.emplace_back(std::make_unique()).get(); + info->name = sourceName; + // keep sorted by name + std::sort( + m_sourceInfo.begin(), m_sourceInfo.end(), + [](const auto& a, const auto& b) { return a->name < b->name; }); + } else { + info = it->get(); + } + + // update NTSourceInfo + std::string_view key = parts[2]; + if ((event.flags & NT_EVENT_PUBLISH) != 0) { + if (key == "description" && topicInfo->type == NT_STRING) { + info->descTopic = topicInfo->topic; + m_topicMap.try_emplace(topicInfo->topic, info); + } else if (key == "connected" && topicInfo->type == NT_BOOLEAN) { + info->connTopic = topicInfo->topic; + m_topicMap.try_emplace(topicInfo->topic, info); + } else if (key == "streams" && topicInfo->type == NT_STRING_ARRAY) { + info->streamsTopic = topicInfo->topic; + m_topicMap.try_emplace(topicInfo->topic, info); + } + } else if ((event.flags & NT_EVENT_UNPUBLISH) != 0) { + if (key == "description") { + m_topicMap.erase(info->connTopic); + info->descTopic = 0; + } else if (key == "connected") { + m_topicMap.erase(info->connTopic); + info->connTopic = 0; + info->connected = false; + } else if (key == "streams") { + m_topicMap.erase(info->connTopic); + info->streamsTopic = 0; + } + } + } + + if (nt::ValueEventData* valueData = event.GetValueEventData()) { + auto it = m_topicMap.find(valueData->topic); + if (it != m_topicMap.end()) { + NTSourceInfo* info = it->getSecond(); + if (valueData->topic == info->descTopic && + valueData->value.IsString()) { + info->description = valueData->value.GetString(); + } else if (valueData->topic == info->connTopic && + valueData->value.IsBoolean()) { + info->connected = valueData->value.GetBoolean(); + } else if (valueData->topic == info->streamsTopic && + valueData->value.IsStringArray()) { + info->streams.clear(); + for (auto&& stream : valueData->value.GetStringArray()) { + if (wpi::starts_with(stream, "mjpg:")) { + info->streams.emplace_back(wpi::drop_front(stream, 5)); + } + } + if (info->camera) { + info->camera->SetUrls(info->streams); + } + } + } + } + } +} + CameraProvider::CameraProvider(Storage& storage) : WindowManager{storage} { storage.SetCustomApply([this] { // loop over windows diff --git a/glass/src/libcs/native/cpp/CameraSource.cpp b/glass/src/libcs/native/cpp/CameraSource.cpp new file mode 100644 index 00000000000..a2df1094311 --- /dev/null +++ b/glass/src/libcs/native/cpp/CameraSource.cpp @@ -0,0 +1,116 @@ +// 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/camera/CameraSource.h" + +#include +#include +#include + +using namespace glass; + +wpi::sig::Signal CameraSource::sourceCreated; +wpi::StringMap CameraSource::sources; + +CameraSource::~CameraSource() { + Stop(); + if (m_frameThread.joinable()) { + m_frameThread.join(); + } + delete m_latestFrame.load(); + for (auto frame : m_sharedFreeList) { + delete frame; + } + for (auto frame : m_sourceFreeList) { + delete frame; + } +} + +void CameraSource::UpdateTexture() { + // create or update texture when we get a new frame + if (auto frame = m_latestFrame.exchange(nullptr)) { + if (!m_tex || frame->cols != m_tex.GetWidth() || + frame->rows != m_tex.GetHeight()) { + m_tex = wpi::gui::Texture(wpi::gui::kPixelRGBA, frame->cols, frame->rows, + frame->data); + } else { + m_tex.Update(frame->data); + } + // put back on shared freelist + std::scoped_lock lock(m_sharedFreeListMutex); + m_sharedFreeList.emplace_back(frame); + } +} + +void CameraSource::Start() { + if (m_frameThread.joinable()) { + return; + } + m_stopCamera = false; + m_frameThread = std::thread([this, source = m_source] { + cs::CvSink cvSink{fmt::format("{}_view", m_id), cs::VideoMode::kBGR}; + cvSink.SetSource(source); + cv::Mat frame; + while (!m_stopCamera) { + // get frame from camera + uint64_t time = cvSink.GrabFrame(frame, 0.25); + if (m_stopCamera) { + break; + } + + cv::Mat* out = AllocMat(); + + if (time == 0) { + *out = cv::Mat::zeros(16, 16, CV_8UC4); + } else { + // convert to RGBA + cv::cvtColor(frame, *out, cv::COLOR_BGR2RGBA); + } + + // make available + auto prev = m_latestFrame.exchange(out); + + // put prev on free list + if (prev) { + m_sourceFreeList.emplace_back(prev); + } + } + }); +} + +void CameraSource::Stop() { + m_stopCamera = true; +} + +CameraSource* CameraSource::Find(std::string_view id) { + auto it = sources.find(id); + if (it == sources.end()) { + return nullptr; + } + return it->second; +} + +cv::Mat* CameraSource::AllocMat() { + // get or create a mat, prefer sourceFreeList over sharedFreeList + cv::Mat* out; + if (!m_sourceFreeList.empty()) { + out = m_sourceFreeList.back(); + m_sourceFreeList.pop_back(); + } else { + { + std::scoped_lock lock(m_sharedFreeListMutex); + for (auto mat : m_sharedFreeList) { + m_sourceFreeList.emplace_back(mat); + } + m_sharedFreeList.clear(); + } + if (!m_sourceFreeList.empty()) { + out = m_sourceFreeList.back(); + m_sourceFreeList.pop_back(); + } else { + out = new cv::Mat; + } + } + return out; +} diff --git a/glass/src/libcs/native/include/glass/camera/CameraProvider.h b/glass/src/libcs/native/include/glass/camera/CameraProvider.h index cef527c4264..56d6ac3f495 100644 --- a/glass/src/libcs/native/include/glass/camera/CameraProvider.h +++ b/glass/src/libcs/native/include/glass/camera/CameraProvider.h @@ -4,6 +4,9 @@ #pragma once +#include +#include + #include "glass/WindowManager.h" namespace glass { @@ -11,11 +14,28 @@ namespace glass { class CameraProvider : private WindowManager { public: explicit CameraProvider(Storage& storage); + CameraProvider(Storage& storage, nt::NetworkTableInstance inst); ~CameraProvider() override; using WindowManager::GlobalInit; void DisplayMenu() override; + + private: + struct SourceInfo { + std::string name; + std::string description; + bool connected = false; + + NT_Topic descTopic{0}; + NT_Topic connTopic{0}; + NT_Topic streamsTopic{0}; + std::vector streams; + }; + + nt::NetworkTableInstance m_inst; + nt::NetworkTableListenerPoller m_poller; + NT_Listener m_listener{0}; }; } // namespace glass diff --git a/glass/src/libcs/native/include/glass/camera/CameraSource.h b/glass/src/libcs/native/include/glass/camera/CameraSource.h new file mode 100644 index 00000000000..87b2d3f5b4b --- /dev/null +++ b/glass/src/libcs/native/include/glass/camera/CameraSource.h @@ -0,0 +1,96 @@ +// 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 +#include +#include +#include +#include +#include + +namespace glass { + +/** + * A data source for camera data. + */ +class CameraSource { + public: + explicit CameraSource(std::string_view id, cs::VideoSource source) + : m_id{id}, m_source{source} {} + virtual ~CameraSource(); + + CameraSource(const CameraSource&) = delete; + CameraSource& operator=(const CameraSource&) = delete; + + const char* GetId() const { return m_id.c_str(); } + + void UpdateTexture(); + + wpi::gui::Texture& GetTexture() { return m_tex; } + + void Start(); + void Stop(); + bool IsRunning() const { return m_frameThread.joinable(); } + + /** + * Get the source handle. + */ + cs::VideoSource& GetSource() { return m_source; } + + /** + * Get the actual FPS. + * + * @return Actual FPS averaged over a 1 second period. + */ + double GetActualFPS() { return m_source.GetActualFPS(); } + + /** + * Get the data rate (in bytes per second). + * + *

SetTelemetryPeriod() must be called for this to be valid. + * + * @return Data rate averaged over a 1 second period. + */ + double GetActualDataRate() { return m_source.GetActualDataRate(); } + + // drag source helpers + void LabelText(const char* label, const char* fmt, ...) const IM_FMTARGS(3); + void LabelTextV(const char* label, const char* fmt, va_list args) const + IM_FMTLIST(3); + void EmitDrag(ImGuiDragDropFlags flags = 0) const; + + static CameraSource* Find(std::string_view id); + + static wpi::sig::Signal sourceCreated; + + private: + cv::Mat* AllocMat(); + + static wpi::StringMap sources; + + std::string m_id; + cs::VideoSource m_source; + + std::atomic m_latestFrame{nullptr}; + std::vector m_sharedFreeList; + wpi::spinlock m_sharedFreeListMutex; + std::vector m_sourceFreeList; + std::atomic m_stopCamera{false}; + std::thread m_frameThread; + + wpi::gui::Texture m_tex; +}; + +} // namespace glass