diff --git a/CMakeLists.txt b/CMakeLists.txt index bc902fb..50cd207 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,7 +33,8 @@ set(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) target_link_libraries(imgui PUBLIC glfw OpenGL::GL) -add_executable(${PROJECT_NAME} src/rig_reconfigure.cpp src/service_wrapper.cpp src/parameter_tree.cpp external/lodepng/lodepng.cpp) +add_executable(${PROJECT_NAME} src/rig_reconfigure.cpp src/service_wrapper.cpp src/parameter_tree.cpp + src/utils.cpp src/node_window.cpp src/parameter_window.cpp external/lodepng/lodepng.cpp) # uncomment for checking the executable with tsan #target_compile_options(${PROJECT_NAME} PRIVATE -g -fsanitize=thread) diff --git a/include/node_window.hpp b/include/node_window.hpp new file mode 100644 index 0000000..7d8ed50 --- /dev/null +++ b/include/node_window.hpp @@ -0,0 +1,26 @@ +/** + * @file node_window.hpp + * @author Dominik Authaler + * @date 13.01.2024 + * + * Code related to the node window within the graphical user interface. + */ + +#ifndef RIG_RECONFIGURE_NODE_WINDOW_HPP +#define RIG_RECONFIGURE_NODE_WINDOW_HPP + +#include "utils.hpp" +#include "service_wrapper.hpp" + +/** + * Renders the window for the node selection. + * @param[in] windowName Window name. + * @param[in] nodeNames List with the available nodes. + * @param[in, out] serviceWrapper Service wrapper for issuing ROS requests. + * @param[in, out] selectedNode Full name of the currently selected node. + * @param[in, out] status Status. + */ +void renderNodeWindow(const char *windowName, const std::vector &nodeNames, + ServiceWrapper &serviceWrapper, std::string &selectedNode, Status &status); + +#endif //RIG_RECONFIGURE_NODE_WINDOW_HPP diff --git a/include/parameter_tree.hpp b/include/parameter_tree.hpp index 80c9c0a..2312af8 100644 --- a/include/parameter_tree.hpp +++ b/include/parameter_tree.hpp @@ -62,8 +62,9 @@ class ParameterTree { void add(const ROSParameter ¶meter); void clear(); - std::shared_ptr getRoot(); + [[nodiscard]] std::shared_ptr getRoot(); [[nodiscard]] std::size_t getMaxParamNameLength() const; + [[nodiscard]] std::string getAppliedFilter() const; [[nodiscard]] ParameterTree filter(const std::string &filterString) const; @@ -81,6 +82,8 @@ class ParameterTree { // bookkeeping for a nicer visualization std::size_t maxParamNameLength = 0; + + std::string appliedFilter; }; #endif // RIG_RECONFIGURE_PARAMETER_TREE_HPP diff --git a/include/parameter_window.hpp b/include/parameter_window.hpp new file mode 100644 index 0000000..dc226aa --- /dev/null +++ b/include/parameter_window.hpp @@ -0,0 +1,32 @@ +/** + * @file parameter_window.hpp + * @author Dominik Authaler + * @date 13.01.2024 + * + * Code related to the parameter window within the graphical user interface. + */ + +#ifndef RIG_RECONFIGURE_PARAMETER_WINDOW_HPP +#define RIG_RECONFIGURE_PARAMETER_WINDOW_HPP + +#include +#include + +#include "parameter_tree.hpp" +#include "service_wrapper.hpp" +#include "utils.hpp" + +/** + * Renders the window for the parameter modification. + * @param[in] windowName Window name. + * @param[in] curSelectedNode Name of the currently selected node. + * @param[in, out] serviceWrapper Service wrapper for issuing ROS requests. + * @param[in, out] filteredParameterTree Filtered parameter tree. + * @param[in, out] filter Filter string input. + * @param[in, out] status Status for displaying errors. + */ +void renderParameterWindow(const char *windowName, const std::string &curSelectedNode, + ServiceWrapper &serviceWrapper, ParameterTree &filteredParameterTree, + std::string &filter, Status &status); + +#endif //RIG_RECONFIGURE_PARAMETER_WINDOW_HPP diff --git a/include/utils.hpp b/include/utils.hpp new file mode 100644 index 0000000..89fff7c --- /dev/null +++ b/include/utils.hpp @@ -0,0 +1,68 @@ +/** + * @file utils.hpp + * @author Dominik Authaler + * @date 12.01.2024 + * + * Collection of utility functions. + */ + +#ifndef RIG_RECONFIGURE_UTILS_HPP +#define RIG_RECONFIGURE_UTILS_HPP + +#include +#include +#include + +#include // will drag system OpenGL headers + +struct Status { + enum class Type { NONE, NO_NODES_AVAILABLE, PARAMETER_CHANGED, SERVICE_TIMEOUT }; + + Type type = Type::NONE; + std::string text; +}; + +/** + * Utility imgui function for partly highlighted text. + * @param text String which should be displayed. + * @param start Starting index of the highlighted part. + * @param end End index of the highlighted part. + * @param highlightColor Color used for the highlighted part, the remaining text is displayed using the default text + * color. + */ +void highlightedText(const std::string &text, std::size_t start, std::size_t end, + const ImVec4 &highlightColor); + +/** + * Utility imgui function for partly highlighted text which can be selected. + * @param text String which should be displayed. + * @param start Starting index of the highlighted part. + * @param end End index of the highlighted part. + * @param highlightColor Color used for the highlighted part, the remaining text is displayed using the default text + * color. + */ +bool highlightedSelectableText(const std::string &text, std::size_t start, std::size_t end, + const ImVec4 &highlightColor); + +/** + * Searches for the resource directory. + * @param execPath Executable path. + * @return Path to the resource directory. + */ +std::filesystem::path findResourcePath(const std::string &execPath); + +/** + * Loads an icon for the provided window. + * @param windowPtr Window for which the icon should be loaded. + * @param resourcePath Path to the icon data. + */ +void loadWindowIcon(GLFWwindow *windowPtr, const std::filesystem::path &resourcePath); + +/** + * Prints the corresponding error. + * @param error Error code. + * @param description Detailed error description. + */ +void glfw_error_callback(int error, const char *description); + +#endif //RIG_RECONFIGURE_UTILS_HPP diff --git a/src/node_window.cpp b/src/node_window.cpp new file mode 100644 index 0000000..8eddfea --- /dev/null +++ b/src/node_window.cpp @@ -0,0 +1,196 @@ +/** + * @file node_window.cpp + * @author Dominik Authaler + * @date 13.01.2024 + * + * Code related to the node window within the graphical user interface. + */ + +#include "node_window.hpp" + +#include +#include + +// height of the box in which the nodes are visualized +static constexpr auto BOX_HEIGHT = 500; + +// utility structures and functions (definitions mainly follow at the end of the file) +struct TreeNode { + std::string name; // node name (leaf node) / namespace (other) + std::string fullName; // full node name for easier usage + + std::vector> children; +}; + +class NodeTree { + public: + explicit NodeTree(const std::vector &nodes); + + std::shared_ptr getRoot(); + + private: + struct SortComparator { + inline bool operator() (const std::shared_ptr& node1, const std::shared_ptr& node2); + }; + + /** + * Adds a new node within the node tree. + * @param curNode Root node of the (sub-)tree. + * @param name (Partial) node name that is considered for inserting the node within the tree. + * @param fullName Full name of the new node (stored for convenient access to selected nodes). + */ + void addNode(const std::shared_ptr& curNode, const std::string &name, const std::string &fullName); + + /** + * Reorders children of nodes: + * - leaf nodes before inner nodes + * - alphabetically within same groups + */ + void sortAlphabetically(const std::shared_ptr& curNode); + + std::shared_ptr root; +}; + +void visualizeNodeTree(const std::shared_ptr& root, std::string &selectedNode); + +void renderNodeWindow(const char *windowName, const std::vector &nodeNames, + ServiceWrapper &serviceWrapper, std::string &selectedNode, Status &status) { + ImGui::Begin(windowName); + + if (nodeNames.empty()) { + ImGui::Text("No nodes available!"); + } else { + ImGui::Text("Available nodes:"); + + // organize nodes as a (sorted) tree and visualize them in a foldable structure + NodeTree tree(nodeNames); + + // the list box creates a highlighted area in which scrolling is possible + if (ImGui::BeginListBox("##Nodes", ImVec2(-1, BOX_HEIGHT))) { + visualizeNodeTree(tree.getRoot(), selectedNode); + ImGui::EndListBox(); + } + } + + if (ImGui::Button("Refresh")) { + serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_NAMES)); + + if (status.type == Status::Type::SERVICE_TIMEOUT) { + status.text.clear(); + status.type = Status::Type::NONE; + } + } + + ImGui::End(); +} + +// implementation of utility + member functions +NodeTree::NodeTree(const std::vector &nodes) : root(std::make_shared()) { + for (const auto &node : nodes) { + // we ignore the leading slash for building the tree + addNode(root, node.substr(1), node); + } + + sortAlphabetically(root); +} + +std::shared_ptr NodeTree::getRoot() { + return root; +} + +void NodeTree::sortAlphabetically(const std::shared_ptr& curNode) { + for (const auto &child : curNode->children) { + sortAlphabetically(child); + } + + std::sort(curNode->children.begin(), curNode->children.end(), SortComparator()); +} + +void NodeTree::addNode(const std::shared_ptr &curNode, const std::string &name, const std::string &fullName) { + + auto prefixEnd = name.find('/'); + if (prefixEnd == std::string::npos) { + curNode->children.emplace_back(std::make_shared(TreeNode{name, fullName})); + return; + } + + // extract first prefix and find corresponding node + auto prefix = name.substr(0, prefixEnd); + auto remainingName = name.substr(prefixEnd + 1); + + std::shared_ptr nextNode = nullptr; + for (const auto &child : curNode->children) { + if (child->name.starts_with(prefix)) { + nextNode = child; + break; + } + } + + if (nextNode == nullptr) { + nextNode = std::make_shared(TreeNode{name, fullName}); + curNode->children.emplace_back(nextNode); + } else { + // found an existing node, check whether we have to subdivide it + const auto idx = name.find('/'); + if (nextNode->children.empty() && idx != std::string::npos && nextNode->name.find('/') != std::string::npos) { + // nextNode is leaf with prefix (namespace with single child is collapsed) + auto nextNodePrefix = nextNode->name.substr(0, idx); + auto nextNodeRemainingName = nextNode->name.substr(idx + 1); + + nextNode->children.emplace_back(std::make_shared(TreeNode{nextNodeRemainingName, nextNode->fullName})); + + nextNode->name = nextNodePrefix; + nextNode->fullName.clear(); + } else if (nextNode->children.empty() && idx != std::string::npos && nextNode->name == prefix) { + // nextNode is a leaf node with same name as next namespace token + // create sibling to nextNode with same name, and add node below that + nextNode = std::make_shared(TreeNode{prefix, fullName}); + curNode->children.emplace_back(nextNode); + } + + addNode(nextNode, remainingName, fullName); + } +} + +bool NodeTree::SortComparator::operator()(const std::shared_ptr &node1, + const std::shared_ptr &node2) { + bool res; + + if (node1->children.empty() && !node2->children.empty()) { + res = true; + } else if (!node1->children.empty() && node2->children.empty()) { + res = false; + } else { + res = (node1->name < node2->name); + } + + return res; +} + +void visualizeNodeTree(const std::shared_ptr& root, std::string &selectedNode) { + if (root->children.empty()) { + // leaf node + // push "leaf" to id stack to prevent ID collision between node and namespace with same name + ImGui::PushID("leaf"); + if (ImGui::Selectable(root->name.c_str(), selectedNode == root->fullName)) { + + selectedNode = root->fullName; + } + ImGui::PopID(); + } else { + // inner node + if (!root->name.empty()) { + if (ImGui::TreeNode(root->name.c_str())) { + for (const auto &child : root->children) { + visualizeNodeTree(child, selectedNode); + } + + ImGui::TreePop(); + } + } else { + for (const auto &child : root->children) { + visualizeNodeTree(child, selectedNode); + } + } + } +} diff --git a/src/parameter_tree.cpp b/src/parameter_tree.cpp index a2c80eb..91a90d3 100644 --- a/src/parameter_tree.cpp +++ b/src/parameter_tree.cpp @@ -25,6 +25,7 @@ void ParameterTree::add(const ROSParameter ¶meter) { } void ParameterTree::clear() { root = std::make_shared(); + appliedFilter.clear(); } void ParameterTree::add(const std::shared_ptr &curNode, const TreeElement ¶meter) { @@ -63,10 +64,15 @@ std::size_t ParameterTree::getMaxParamNameLength() const { return maxParamNameLength; } +std::string ParameterTree::getAppliedFilter() const { + return appliedFilter; +} + ParameterTree ParameterTree::filter(const std::string &filterString) const { ParameterTree filteredTree; filteredTree.maxParamNameLength = maxParamNameLength; + filteredTree.appliedFilter = filterString; // first pass: filter all parameters filter(filteredTree.getRoot(), root, filterString); diff --git a/src/parameter_window.cpp b/src/parameter_window.cpp new file mode 100644 index 0000000..c73d4b4 --- /dev/null +++ b/src/parameter_window.cpp @@ -0,0 +1,245 @@ +/** + * @file parameter_window.hpp + * @author Dominik Authaler + * @date 13.01.2024 + * + * Code related to the parameter window within the graphical user interface. + */ + +#include "parameter_window.hpp" + +#include // necessary for the tree node manipulation +#include +#include + +#include "utils.hpp" + +/// Minimum width specified for text input fields. +constexpr auto MIN_INPUT_TEXT_FIELD_WIDTH = 100; +/// Width of the window reserved for padding (e.g. between parameter name and input field) in case the width of the +/// input field is scaled using the window width. +constexpr auto TEXT_INPUT_FIELD_PADDING = 100; +/// Reduction of the text field width per nesting level (necessary to avoid input field spanning across the window +/// borders) +constexpr auto TEXT_FIELD_WIDTH_REDUCTION_PER_LEVEL = 22; +constexpr auto FILTER_INPUT_TEXT_FIELD_WIDTH = 250; +constexpr auto FILTER_HIGHLIGHTING_COLOR = ImVec4(1, 0, 0, 1); +constexpr auto TEXT_INPUT_EDITING_END_CHARACTERS = "\n"; + +static std::set visualizeParameters(ServiceWrapper &serviceWrapper, + const std::shared_ptr ¶meterNode, + std::size_t maxParamLength, std::size_t textfieldWidth, + const std::string &filterString, bool expandAll = false); + +void renderParameterWindow(const char *windowName, const std::string &curSelectedNode, + ServiceWrapper &serviceWrapper, ParameterTree &filteredParameterTree, std::string &filter, + Status &status) { + // unfortunately DearImGui doesn't provide any option to collapse tree nodes recursively, hence, we need to keep + // track of the ID of each open tree node (across function calls, since a tree node stays open even if + // the parent node is closed) + static std::set treeNodeIDs; + static std::string displayedNodeName; + + if (displayedNodeName != curSelectedNode) { + displayedNodeName = curSelectedNode; + treeNodeIDs.clear(); + } + + ImGui::Begin(windowName); + + bool expandAllParameters = false; + + const auto curWindowWidth = static_cast(ImGui::GetWindowSize().x); + + if (!curSelectedNode.empty()) { + ImGui::Text("Parameters of '%s'", curSelectedNode.c_str()); + ImGui::Dummy(ImVec2(0.0F, 5.0F)); + + if (ImGui::Button("Reload parameters")) { + serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_PARAMETERS)); + } + + ImGui::SameLine(); + + if (ImGui::Button("Expand all")) { + expandAllParameters = true; + } + + ImGui::SameLine(); + + if (ImGui::Button("Collapse all")) { + for (const auto id : treeNodeIDs) { + ImGui::TreeNodeSetOpen(id, false); + } + treeNodeIDs.clear(); + } + + ImGui::Dummy(ImVec2(0.0F, 10.0F)); + + ImGui::AlignTextToFramePadding(); + ImGui::Text("Filter: "); + ImGui::SameLine(); + ImGui::PushItemWidth(FILTER_INPUT_TEXT_FIELD_WIDTH); + ImGui::InputText("##Filter", &filter, ImGuiInputTextFlags_CharsNoBlank); + ImGui::PopItemWidth(); + ImGui::SameLine(); + if (ImGui::Button("Clear")) { + filter = ""; + } + + ImGui::Dummy(ImVec2(0.0F, 10.0F)); + + const auto maxParamLength = filteredParameterTree.getMaxParamNameLength(); + const auto textfieldWidth = std::max(MIN_INPUT_TEXT_FIELD_WIDTH, curWindowWidth - static_cast(maxParamLength) - TEXT_INPUT_FIELD_PADDING); + + const auto ids = visualizeParameters(serviceWrapper, filteredParameterTree.getRoot(), + maxParamLength, textfieldWidth, filteredParameterTree.getAppliedFilter(), + expandAllParameters); + treeNodeIDs.insert(ids.begin(), ids.end()); + + if (status.type == Status::Type::NO_NODES_AVAILABLE) { + status.text.clear(); + status.type = Status::Type::NONE; + } + } else { + ImGui::Text("Please select a node first!"); + } + + ImGui::End(); +} + +std::set visualizeParameters(ServiceWrapper &serviceWrapper, + const std::shared_ptr ¶meterNode, + const std::size_t maxParamLength, + const std::size_t textfieldWidth, + const std::string &filterString, + const bool expandAll) { + // required to store which of the text input fields is 'dirty' (has changes which have not yet been propagated to + // the ROS service (because editing has not yet been finished)) + // --> since ImGui only allows a single active input field storing the path of the corresponding parameter is enough + static std::string dirtyTextInput; + + std::set nodeIDs; + auto *window = ImGui::GetCurrentWindow(); + + if (parameterNode == nullptr || (parameterNode->parameters.empty() && parameterNode->subgroups.empty())) { + if (!filterString.empty()) { + ImGui::Text("This node doesn't seem to have any parameters\nmatching the filter!"); + } else { + ImGui::Text("This node doesn't seem to have any parameters!"); + } + + return {}; + } + + for (auto &[name, fullPath, value, highlightingStart, highlightingEnd] : parameterNode->parameters) { + std::string identifier = "##" + name; + + // simple 'space' padding to avoid the need for a more complex layout with columns (the latter is still desired + // :D) + std::string padding; + if (name.length() < maxParamLength) { + padding = std::string(maxParamLength - name.length(), ' '); + } + + ImGui::AlignTextToFramePadding(); + + if (highlightingStart.has_value() && highlightingEnd.has_value()) { + highlightedText(name, highlightingStart.value(), highlightingEnd.value(), FILTER_HIGHLIGHTING_COLOR); + } else { + ImGui::Text("%s", name.c_str()); + } + + ImGui::SameLine(0, 0); + ImGui::Text("%s", padding.c_str()); + + ImGui::SameLine(); + ImGui::PushItemWidth(static_cast(textfieldWidth)); + + if (std::holds_alternative(value)) { + ImGui::DragScalar(identifier.c_str(), ImGuiDataType_Double, &std::get(value), 1.0F, nullptr, + nullptr, "%.6g"); + if (ImGui::IsItemDeactivatedAfterEdit()) { + serviceWrapper.pushRequest( + std::make_shared(ROSParameter(fullPath, value))); + } + } else if (std::holds_alternative(value)) { + if (ImGui::Checkbox(identifier.c_str(), &std::get(value))) { + serviceWrapper.pushRequest( + std::make_shared(ROSParameter(fullPath, value))); + } + } else if (std::holds_alternative(value)) { + ImGui::DragInt(identifier.c_str(), &std::get(value)); + if (ImGui::IsItemDeactivatedAfterEdit()) { + serviceWrapper.pushRequest( + std::make_shared(ROSParameter(fullPath, value))); + } + } else if (std::holds_alternative(value)) { + // Set to true when enter is pressed + bool flush = false; + + // Note: ImGui provides an option to provide only callbacks on enter, but we additionally need the + // information whether the text field is 'dirty', hence, we need to check for 'enter' + // by ourselves + if (ImGui::InputText(identifier.c_str(), &std::get(value))) { + dirtyTextInput = fullPath; + + auto &str = std::get(value); + + // check if last character indicates the end of the editing + if (str.ends_with(TEXT_INPUT_EDITING_END_CHARACTERS)) { + flush = true; + str.pop_back(); + } + } + + // Second condition: InputText focus lost + if (flush || (!ImGui::IsItemActive() && dirtyTextInput == fullPath)) { + dirtyTextInput.clear(); + serviceWrapper.pushRequest( + std::make_shared(ROSParameter(fullPath, value))); + } + } + ImGui::PopItemWidth(); + } + + if (!parameterNode->subgroups.empty()) { + for (const auto &subgroup : parameterNode->subgroups) { + if (expandAll) { + ImGui::SetNextItemOpen(true); + } + + const auto label = "##" + subgroup->prefix; + + // this is hacky, we need the ID of the TreeNode in order to access the memory for collapsing it + const auto nodeID = window->GetID(label.c_str()); + nodeIDs.insert(nodeID); + + bool open = ImGui::TreeNode(label.c_str()); + + ImGui::SameLine(); + bool textClicked = false; + if (subgroup->prefixSearchPatternStart.has_value() && subgroup->prefixSearchPatternEnd.has_value()) { + textClicked = highlightedSelectableText(subgroup->prefix, subgroup->prefixSearchPatternStart.value(), + subgroup->prefixSearchPatternEnd.value(), + FILTER_HIGHLIGHTING_COLOR); + } else { + textClicked = ImGui::Selectable((subgroup->prefix).c_str()); + } + + if (textClicked) { + ImGui::TreeNodeSetOpen(nodeID, !open); + } + + if (open) { + const auto newWidth = textfieldWidth - TEXT_FIELD_WIDTH_REDUCTION_PER_LEVEL; + auto subIDs = visualizeParameters(serviceWrapper, subgroup, maxParamLength, newWidth, + filterString, expandAll); + nodeIDs.insert(subIDs.begin(), subIDs.end()); + ImGui::TreePop(); + } + } + } + + return nodeIDs; +} diff --git a/src/rig_reconfigure.cpp b/src/rig_reconfigure.cpp index e722c18..a739d97 100644 --- a/src/rig_reconfigure.cpp +++ b/src/rig_reconfigure.cpp @@ -8,57 +8,31 @@ #include // Will drag system OpenGL headers -#include -#include -#include #include -#include +#include +#include +#include +#include #include -#include "imgui.h" -#include "imgui_impl_glfw.h" -#include "imgui_impl_opengl3.h" -#include "imgui_internal.h" -#include "lodepng.h" -#include "misc/cpp/imgui_stdlib.h" -#include "parameter_tree.hpp" #include "service_wrapper.hpp" +#include "utils.hpp" +#include "lodepng.h" +#include "node_window.hpp" +#include "parameter_window.hpp" using namespace std::chrono_literals; -/// Minimum width specified for text input fields. -constexpr auto MIN_INPUT_TEXT_FIELD_WIDTH = 100; -/// Width of the window reserved for padding (e.g. between parameter name and input field) in case the width of the -/// input field is scaled using the window width. -constexpr auto TEXT_INPUT_FIELD_PADDING = 100; -/// Reduction of the text field width per nesting level (necessary to avoid input field spanning across the window -/// borders) -constexpr auto TEXT_FIELD_WIDTH_REDUCTION_PER_LEVEL = 22; -constexpr auto FILTER_INPUT_TEXT_FIELD_WIDTH = 250; -constexpr auto FILTER_HIGHLIGHTING_COLOR = ImVec4(1, 0, 0, 1); constexpr auto STATUS_WARNING_COLOR = ImVec4(1, 0, 0, 1); constexpr auto NODES_AUTO_REFRESH_INTERVAL = 1s; // unit: seconds constexpr auto DESIRED_FRAME_RATE = 30; constexpr std::chrono::duration DESIRED_FRAME_DURATION_MS = 1000ms / DESIRED_FRAME_RATE; -constexpr auto TEXT_INPUT_EDITING_END_CHARACTERS = "\n"; -enum class StatusTextType { NONE, NO_NODES_AVAILABLE, PARAMETER_CHANGED, SERVICE_TIMEOUT }; - -static void glfw_error_callback(int error, const char *description) { - fprintf(stderr, "Glfw Error %d: %s\n", error, description); -} +// window names +constexpr auto NODE_WINDOW_NAME = "Nodes"; +constexpr auto STATUS_WINDOW_NAME = "Status"; +constexpr auto PARAMETER_WINDOW_NAME = "Parameters"; -static std::set visualizeParameters(ServiceWrapper &serviceWrapper, - const std::shared_ptr ¶meterNode, - std::size_t maxParamLength, std::size_t textfieldWidth, - const std::string &filterString, bool expandAll = false); -static void highlightedText(const std::string &text, std::size_t start, std::size_t end, - const ImVec4 &highlightColor = FILTER_HIGHLIGHTING_COLOR); -static bool highlightedSelectableText(const std::string &text, std::size_t start, std::size_t end, - const ImVec4 &highlightColor = FILTER_HIGHLIGHTING_COLOR); - -static std::filesystem::path findResourcePath(const std::string &execPath); -static void loadWindowIcon(GLFWwindow *windowPtr, const std::filesystem::path &resourcePath); static void renderInfoWindow(bool *showInfoWindow, const std::filesystem::path &resourcePath); int main(int argc, char *argv[]) { @@ -86,9 +60,11 @@ int main(int argc, char *argv[]) { glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0); // Create window with graphics context - GLFWwindow *window = glfwCreateWindow(600, 800, "Parameter modification editor", NULL, NULL); - if (window == NULL) + GLFWwindow *window = glfwCreateWindow(600, 800, "Parameter modification editor", + nullptr, nullptr); + if (window == nullptr) { return 1; + } glfwMakeContextCurrent(window); glfwSwapInterval(1); // Enable vsync @@ -121,22 +97,16 @@ int main(int argc, char *argv[]) { ImGui_ImplGlfw_InitForOpenGL(window, true); ImGui_ImplOpenGL3_Init(glsl_version); - int selectedIndex = -1; - int nodeNameIndex = -1; std::vector nodeNames; - std::string curSelectedNode; - std::string status; - StatusTextType statusType = StatusTextType::NONE; + std::string selectedNode; // name of the node which has been selected in the current iteration + std::string curDisplayedNode; // name of the node for which parameters are currently displayed + Status status; ParameterTree parameterTree; // tree with all parameters ParameterTree filteredParameterTree; // parameter tree after the application of the filter string bool reapplyFilter = true; std::string filter; // current filter string of the text input field - std::string currentFilterString; // currently active filter string bool autoRefreshNodes = true; auto lastNodeRefreshTime = std::chrono::system_clock::now(); - // unfortunately DearImGui doesn't provide any option to collapse tree nodes recursively, hence, we need to keep - // track of all the ID of each open tree node - std::set treeNodeIDs; // request available nodes on startup serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_NAMES)); @@ -149,8 +119,6 @@ int main(int argc, char *argv[]) { // Main loop while (!glfwWindowShouldClose(window)) { // these variables are only relevant for a single iteration - bool expandAllParameters = false; - const auto frame_start = std::chrono::high_resolution_clock::now(); // check the response queue @@ -163,8 +131,8 @@ int main(int argc, char *argv[]) { nodeNames = std::dynamic_pointer_cast(response)->nodeNames; if (nodeNames.empty()) { - status = "Seems like there are no nodes available!"; - statusType = StatusTextType::NO_NODES_AVAILABLE; + status.text = "Seems like there are no nodes available!"; + status.type = Status::Type::NO_NODES_AVAILABLE; } break; @@ -184,11 +152,11 @@ int main(int argc, char *argv[]) { auto result = std::dynamic_pointer_cast(response); if (result->success) { - status = "Parameter '" + result->parameterName + "' modified successfully!"; + status.text = "Parameter '" + result->parameterName + "' modified successfully!"; } else { - status = "Parameter '" + result->parameterName + "' couldn't be modified!"; + status.text = "Parameter '" + result->parameterName + "' couldn't be modified!"; } - statusType = StatusTextType::PARAMETER_CHANGED; + status.type = Status::Type::PARAMETER_CHANGED; break; } @@ -196,8 +164,8 @@ int main(int argc, char *argv[]) { case Response::Type::SERVICE_TIMEOUT: { auto result = std::dynamic_pointer_cast(response); - status = "Node '" + result->nodeName + "' didn't respond to service call. Maybe the node has died?"; - statusType = StatusTextType::SERVICE_TIMEOUT; + status.text = "Node '" + result->nodeName + "' didn't respond to service call. Maybe the node has died?"; + status.type = Status::Type::SERVICE_TIMEOUT; break; } @@ -205,47 +173,31 @@ int main(int argc, char *argv[]) { } // handle changes of the selected node + died nodes / newly added nodes during the refresh step - const auto nodeNameIterator = std::find(nodeNames.begin(), nodeNames.end(), curSelectedNode); + const auto nodeNameIterator = std::find(nodeNames.begin(), nodeNames.end(), curDisplayedNode); bool nodeStillAvailable = (nodeNameIterator != nodeNames.end()); - bool nameAtIndexChanged = (selectedIndex < nodeNames.size() && curSelectedNode != nodeNames.at(selectedIndex)); - - if (nodeNameIndex == selectedIndex && nameAtIndexChanged && nodeStillAvailable) { - // node list has changed, e.g. because new nodes have been started - // -> selected node does still exist, hence, we simply need to update the selected index - selectedIndex = std::distance(nodeNames.begin(), nodeNameIterator); - nodeNameIndex = selectedIndex; - } else if (nodeNameIndex != selectedIndex) { - // selected node has changed - selectedIndex = nodeNameIndex; - - treeNodeIDs.clear(); - auto selectedNodeName = nodeNames.at(selectedIndex); - - if (selectedNodeName != curSelectedNode) { - curSelectedNode = selectedNodeName; + if (!curDisplayedNode.empty() && !nodeStillAvailable) { + status.text = "Warning: Node '" + curDisplayedNode + "' seems to have died!"; + status.type = Status::Type::SERVICE_TIMEOUT; + } else if (!selectedNode.empty() && selectedNode != curDisplayedNode) { + // selected node has changed + curDisplayedNode = selectedNode; - // query parameters of the node - serviceWrapper.setNodeOfInterest(curSelectedNode); - serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_PARAMETERS)); - } + // query parameters of the node + serviceWrapper.setNodeOfInterest(curDisplayedNode); + serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_PARAMETERS)); // clear warning about died node if one switches to another one - if (statusType == StatusTextType::SERVICE_TIMEOUT) { - status.clear(); - statusType = StatusTextType::NONE; + if (status.type == Status::Type::SERVICE_TIMEOUT) { + status.text.clear(); + status.type = Status::Type::NONE; } - } else if (!curSelectedNode.empty() && - (nodeNames.empty() || nameAtIndexChanged || selectedIndex >= nodeNames.size())) { - status = "Warning: Node '" + curSelectedNode + "' seems to have died!"; - statusType = StatusTextType::SERVICE_TIMEOUT; } - if (reapplyFilter == true || currentFilterString != filter) { + if (reapplyFilter == true || filteredParameterTree.getAppliedFilter() != filter) { reapplyFilter = false; - currentFilterString = filter; - filteredParameterTree = parameterTree.filter(currentFilterString); + filteredParameterTree = parameterTree.filter(filter); } // auto refresh the node list periodically @@ -300,7 +252,7 @@ int main(int argc, char *argv[]) { if (ImGui::BeginMenu("Parameters")) { bool propertyChanged = ImGui::MenuItem("Hide default parameters", nullptr, &ignoreDefaultParameters); serviceWrapper.setIgnoreDefaultParameters(ignoreDefaultParameters); - if (propertyChanged && !curSelectedNode.empty()) { + if (propertyChanged && !curDisplayedNode.empty()) { // Reload parameters if menu item was toggled serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_PARAMETERS)); } @@ -317,7 +269,7 @@ int main(int argc, char *argv[]) { renderInfoWindow(&showInfo, resourcePath); - if (ImGui::DockBuilderGetNode(dockspace_id) == NULL || shouldResetLayout || !configFileExisting) { + if (ImGui::DockBuilderGetNode(dockspace_id) == nullptr || shouldResetLayout || !configFileExisting) { shouldResetLayout = false; configFileExisting = true; ImGui::DockBuilderRemoveNode(dockspace_id); // Clear out existing layout @@ -332,111 +284,24 @@ int main(int argc, char *argv[]) { ImGuiID right = 0; ImGui::DockBuilderSplitNode(top, ImGuiDir_Left, 0.3, &left, &right); - ImGui::DockBuilderDockWindow("Nodes", left); - ImGui::DockBuilderDockWindow("Parameters", right); - ImGui::DockBuilderDockWindow("Status", bottom); + ImGui::DockBuilderDockWindow(NODE_WINDOW_NAME, left); + ImGui::DockBuilderDockWindow(PARAMETER_WINDOW_NAME, right); + ImGui::DockBuilderDockWindow(STATUS_WINDOW_NAME, bottom); ImGui::DockBuilderFinish(dockspace_id); } - ImGui::Begin("Nodes"); - - if (nodeNames.empty()) { - ImGui::Text("No nodes available!"); - } else { - ImGui::Text("Available nodes:"); - - if (ImGui::BeginListBox("##Nodes", ImVec2(-1, 500))) { - for (auto i = 0U; i < nodeNames.size(); ++i) { - const bool isSelected = (nodeNameIndex == i); - if (ImGui::Selectable(nodeNames[i].c_str(), isSelected)) { - nodeNameIndex = i; - } - } - ImGui::EndListBox(); - } - - if (statusType == StatusTextType::NO_NODES_AVAILABLE) { - status.clear(); - statusType = StatusTextType::NONE; - } - } - - if (ImGui::Button("Refresh")) { - serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_NAMES)); - - if (statusType == StatusTextType::SERVICE_TIMEOUT) { - status.clear(); - statusType = StatusTextType::NONE; - } - } - - ImGui::End(); - - ImGui::Begin("Parameters"); - - const auto curWindowWidth = static_cast(ImGui::GetWindowSize().x); - - if (!curSelectedNode.empty()) { - ImGui::Text("Parameters of '%s'", curSelectedNode.c_str()); - ImGui::Dummy(ImVec2(0.0f, 5.0f)); + renderNodeWindow(NODE_WINDOW_NAME, nodeNames, serviceWrapper, selectedNode, status); - if (ImGui::Button("Reload parameters")) { - serviceWrapper.pushRequest(std::make_shared(Request::Type::QUERY_NODE_PARAMETERS)); - } - - ImGui::SameLine(); - - if (ImGui::Button("Expand all")) { - expandAllParameters = true; - } - - ImGui::SameLine(); - - if (ImGui::Button("Collapse all")) { - for (const auto id : treeNodeIDs) { - ImGui::TreeNodeSetOpen(id, false); - } - treeNodeIDs.clear(); - } - - ImGui::Dummy(ImVec2(0.0f, 10.0f)); - - ImGui::AlignTextToFramePadding(); - ImGui::Text("Filter: "); - ImGui::SameLine(); - ImGui::PushItemWidth(FILTER_INPUT_TEXT_FIELD_WIDTH); - ImGui::InputText("##Filter", &filter, ImGuiInputTextFlags_CharsNoBlank); - ImGui::PopItemWidth(); - ImGui::SameLine(); - if (ImGui::Button("Clear")) { - filter = ""; - } - - ImGui::Dummy(ImVec2(0.0f, 10.0f)); - - const auto maxParamLength = filteredParameterTree.getMaxParamNameLength(); - const auto textfieldWidth = std::max(MIN_INPUT_TEXT_FIELD_WIDTH, curWindowWidth - static_cast(maxParamLength) - TEXT_INPUT_FIELD_PADDING); - - const auto ids = visualizeParameters(serviceWrapper, filteredParameterTree.getRoot(), - maxParamLength, textfieldWidth, currentFilterString, - expandAllParameters); - treeNodeIDs.insert(ids.begin(), ids.end()); - - if (statusType == StatusTextType::NO_NODES_AVAILABLE) { - status.clear(); - statusType = StatusTextType::NONE; - } - } else { - ImGui::Text("Please select a node first!"); - } - - ImGui::End(); + // Note: updating the parameter window at least one iteration late is no problem since the parameters + // have to be queried anyway before being able to visualize anything meaningful + renderParameterWindow(PARAMETER_WINDOW_NAME, curDisplayedNode, serviceWrapper, filteredParameterTree, + filter, status); - ImGui::Begin("Status"); - if (statusType == StatusTextType::SERVICE_TIMEOUT) { - ImGui::TextColored(STATUS_WARNING_COLOR, "%s", status.c_str()); + ImGui::Begin(STATUS_WINDOW_NAME); + if (status.type == Status::Type::SERVICE_TIMEOUT) { + ImGui::TextColored(STATUS_WARNING_COLOR, "%s", status.text.c_str()); } else { - ImGui::Text("%s", status.c_str()); + ImGui::Text("%s", status.text.c_str()); } ImGui::End(); @@ -449,7 +314,7 @@ int main(int argc, char *argv[]) { int display_w = 0; glfwGetFramebufferSize(window, &display_w, &display_h); glViewport(0, 0, display_w, display_h); - ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); + ImVec4 clear_color = ImVec4(0.45F, 0.55F, 0.60F, 1.00F); glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); glClear(GL_COLOR_BUFFER_BIT); @@ -483,227 +348,6 @@ int main(int argc, char *argv[]) { rclcpp::shutdown(); } -std::set visualizeParameters(ServiceWrapper &serviceWrapper, - const std::shared_ptr ¶meterNode, - const std::size_t maxParamLength, - const std::size_t textfieldWidth, - const std::string &filterString, - const bool expandAll) { - // required to store which of the text input fields is 'dirty' (has changes which have not yet been propagated to - // the ROS service (because editing has not yet been finished)) - // --> since ImGui only allows a single active input field storing the path of the corresponding parameter is enough - static std::string dirtyTextInput; - - std::set nodeIDs; - auto *window = ImGui::GetCurrentWindow(); - - if (parameterNode == nullptr || (parameterNode->parameters.empty() && parameterNode->subgroups.empty())) { - if (!filterString.empty()) { - ImGui::Text("This node doesn't seem to have any parameters\nmatching the filter!"); - } else { - ImGui::Text("This node doesn't seem to have any parameters!"); - } - - return {}; - } - - for (auto &[name, fullPath, value, highlightingStart, highlightingEnd] : parameterNode->parameters) { - std::string identifier = "##" + name; - - // simple 'space' padding to avoid the need for a more complex layout with columns (the latter is still desired - // :D) - std::string padding; - if (name.length() < maxParamLength) { - padding = std::string(maxParamLength - name.length(), ' '); - } - - ImGui::AlignTextToFramePadding(); - - if (highlightingStart.has_value() && highlightingEnd.has_value()) { - highlightedText(name, highlightingStart.value(), highlightingEnd.value()); - } else { - ImGui::Text("%s", name.c_str()); - } - - ImGui::SameLine(0, 0); - ImGui::Text("%s", padding.c_str()); - - ImGui::SameLine(); - ImGui::PushItemWidth(static_cast(textfieldWidth)); - - if (std::holds_alternative(value)) { - ImGui::DragScalar(identifier.c_str(), ImGuiDataType_Double, &std::get(value), 1.0F, nullptr, - nullptr, "%.6g"); - if (ImGui::IsItemDeactivatedAfterEdit()) { - serviceWrapper.pushRequest( - std::make_shared(ROSParameter(fullPath, value))); - } - } else if (std::holds_alternative(value)) { - if (ImGui::Checkbox(identifier.c_str(), &std::get(value))) { - serviceWrapper.pushRequest( - std::make_shared(ROSParameter(fullPath, value))); - } - } else if (std::holds_alternative(value)) { - ImGui::DragInt(identifier.c_str(), &std::get(value)); - if (ImGui::IsItemDeactivatedAfterEdit()) { - serviceWrapper.pushRequest( - std::make_shared(ROSParameter(fullPath, value))); - } - } else if (std::holds_alternative(value)) { - // Set to true when enter is pressed - bool flush = false; - - // Note: ImGui provides an option to provide only callbacks on enter, but we additionally need the - // information whether the text field is 'dirty', hence, we need to check for 'enter' - // by ourselves - if (ImGui::InputText(identifier.c_str(), &std::get(value))) { - dirtyTextInput = fullPath; - - auto &str = std::get(value); - - // check if last character indicates the end of the editing - if (str.ends_with(TEXT_INPUT_EDITING_END_CHARACTERS)) { - flush = true; - str.pop_back(); - } - } - - // Second condition: InputText focus lost - if (flush || (!ImGui::IsItemActive() && dirtyTextInput == fullPath)) { - dirtyTextInput.clear(); - serviceWrapper.pushRequest( - std::make_shared(ROSParameter(fullPath, value))); - } - } - ImGui::PopItemWidth(); - } - - if (!parameterNode->subgroups.empty()) { - for (const auto &subgroup : parameterNode->subgroups) { - if (expandAll) { - ImGui::SetNextItemOpen(true); - } - - const auto label = "##" + subgroup->prefix; - - // this is hacky, we need the ID of the TreeNode in order to access the memory for collapsing it - const auto nodeID = window->GetID(label.c_str()); - nodeIDs.insert(nodeID); - - bool open = ImGui::TreeNode(label.c_str()); - - ImGui::SameLine(); - bool textClicked = false; - if (subgroup->prefixSearchPatternStart.has_value() && subgroup->prefixSearchPatternEnd.has_value()) { - textClicked = highlightedSelectableText(subgroup->prefix, subgroup->prefixSearchPatternStart.value(), - subgroup->prefixSearchPatternEnd.value()); - } else { - textClicked = ImGui::Selectable((subgroup->prefix).c_str()); - } - - if (textClicked) { - ImGui::TreeNodeSetOpen(nodeID, !open); - } - - if (open) { - const auto newWidth = textfieldWidth - TEXT_FIELD_WIDTH_REDUCTION_PER_LEVEL; - auto subIDs = visualizeParameters(serviceWrapper, subgroup, maxParamLength, newWidth, - filterString, expandAll); - nodeIDs.insert(subIDs.begin(), subIDs.end()); - ImGui::TreePop(); - } - } - } - - return nodeIDs; -} - - -void highlightedText(const std::string &text, std::size_t start, std::size_t end, const ImVec4 &highlightColor) { - if (start == std::string::npos) { - ImGui::Text("%s", text.c_str()); - return; - } - - if (start > 0) { - ImGui::Text("%s", text.substr(0, start).c_str()); - ImGui::SameLine(0, 0); - } - - ImGui::PushStyleColor(ImGuiCol_Text, FILTER_HIGHLIGHTING_COLOR); - ImGui::Text("%s", text.substr(start, end - start).c_str()); - ImGui::PopStyleColor(); - - if (end < text.length() - 1) { - ImGui::SameLine(0, 0); - ImGui::Text("%s", text.substr(end).c_str()); - } -} - -bool highlightedSelectableText(const std::string &text, std::size_t start, std::size_t end, - const ImVec4 &highlightColor) { - bool selected = false; - - if (start == std::string::npos) { - selected |= ImGui::Selectable(text.c_str()); - return selected; - } - - if (start > 0) { - selected |= ImGui::Selectable(text.substr(0, start).c_str()); - ImGui::SameLine(0, 0); - } - - ImGui::PushStyleColor(ImGuiCol_Text, FILTER_HIGHLIGHTING_COLOR); - selected |= ImGui::Selectable(text.substr(start, end - start).c_str()); - ImGui::PopStyleColor(); - - if (end < text.length() - 1) { - ImGui::SameLine(0, 0); - selected |= ImGui::Selectable(text.substr(end).c_str()); - } - - return selected; -} - -std::filesystem::path findResourcePath(const std::string &execPath) { - auto resourcePath = std::filesystem::path(execPath).parent_path().append("resource"); - - try { - // Try getting package share dir via ament, and use that if it succeeds. - resourcePath = ament_index_cpp::get_package_share_directory("rig_reconfigure"); - resourcePath.append("resource"); - } catch (ament_index_cpp::PackageNotFoundError &e) { - std::cerr << "Warning: Error while looking for package share directory: " << e.what() - << "\n"; - } - - return resourcePath; -} - -void loadWindowIcon(GLFWwindow *windowPtr, const std::filesystem::path &resourcePath) { - const auto logoPath = resourcePath / "rig_reconfigure.png"; - - std::vector iconData; - unsigned int width; - unsigned int height; - - unsigned int error = lodepng::decode(iconData, width, height, logoPath.string()); - - if (error == 0) { - GLFWimage icon; - - icon.width = static_cast(width); - icon.height = static_cast(height); - icon.pixels = iconData.data(); - - glfwSetWindowIcon(windowPtr, 1, &icon); - } else { - std::cerr << "Unable to load window icon (decoder error " << error << " - " << lodepng_error_text(error) << ")" - << std::endl; - } -} - void renderInfoWindow(bool *showInfoWindow, const std::filesystem::path &resourcePath) { // load the logo with text only once static GLuint imageTexture; @@ -731,7 +375,8 @@ void renderInfoWindow(bool *showInfoWindow, const std::filesystem::path &resourc #if defined(GL_UNPACK_ROW_LENGTH) && !defined(__EMSCRIPTEN__) glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); #endif - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, imageData.data()); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,static_cast(width), + static_cast(height), 0, GL_RGBA, GL_UNSIGNED_BYTE, imageData.data()); } imageLoaded = true; diff --git a/src/service_wrapper.cpp b/src/service_wrapper.cpp index f5eeea6..cfc9ac2 100644 --- a/src/service_wrapper.cpp +++ b/src/service_wrapper.cpp @@ -3,7 +3,9 @@ * @author Dominik Authaler * @date 22.01.2023 * - * Utility class wrapping all the service related calls. + * Utility class wrapping all the service related calls. This class has been initially inserted in order to allow + * using the tool together with ROS 1 by simple replacing this class. However, as of 01/2024 there are no plans to do + * so. */ #include "service_wrapper.hpp" @@ -124,12 +126,19 @@ void ServiceWrapper::handleRequest(const RequestPtr &request) { continue; } + { + auto tmpclient = node->create_client(serviceName); + if (!tmpclient->service_is_ready()) { + // Service is known, but not ready. + // This happens e.g. if this is the currently selected node, + // so we still have clients for the service, but the node has died. + continue; + } + } + nodeNames.push_back(extractedNodeName); } - // sort nodes alphabetically - std::sort(nodeNames.begin(), nodeNames.end()); - auto response = std::make_shared(std::move(nodeNames)); responseQueue.push(response); diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..9b0da11 --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,105 @@ +/** + * @file utils.cpp + * @author Dominik Authaler + * @date 12.01.2024 + * + * Collection of utility functions. + */ + +#include "utils.hpp" + +#include +#include +#include +#include + +#include "lodepng.h" + +void highlightedText(const std::string &text, std::size_t start, std::size_t end, const ImVec4 &highlightColor) { + if (start == std::string::npos) { + ImGui::Text("%s", text.c_str()); + return; + } + + if (start > 0) { + ImGui::Text("%s", text.substr(0, start).c_str()); + ImGui::SameLine(0, 0); + } + + ImGui::PushStyleColor(ImGuiCol_Text, highlightColor); + ImGui::Text("%s", text.substr(start, end - start).c_str()); + ImGui::PopStyleColor(); + + if (end < text.length() - 1) { + ImGui::SameLine(0, 0); + ImGui::Text("%s", text.substr(end).c_str()); + } +} + +bool highlightedSelectableText(const std::string &text, std::size_t start, std::size_t end, + const ImVec4 &highlightColor) { + bool selected = false; + + if (start == std::string::npos) { + selected |= ImGui::Selectable(text.c_str()); + return selected; + } + + if (start > 0) { + selected |= ImGui::Selectable(text.substr(0, start).c_str()); + ImGui::SameLine(0, 0); + } + + ImGui::PushStyleColor(ImGuiCol_Text, highlightColor); + selected |= ImGui::Selectable(text.substr(start, end - start).c_str()); + ImGui::PopStyleColor(); + + if (end < text.length() - 1) { + ImGui::SameLine(0, 0); + selected |= ImGui::Selectable(text.substr(end).c_str()); + } + + return selected; +} + +std::filesystem::path findResourcePath(const std::string &execPath) { + auto resourcePath = std::filesystem::path(execPath).parent_path().append("resource"); + + try { + // Try getting package share dir via ament, and use that if it succeeds. + resourcePath = ament_index_cpp::get_package_share_directory("rig_reconfigure"); + resourcePath.append("resource"); + } catch (ament_index_cpp::PackageNotFoundError &e) { + std::cerr << "Warning: Error while looking for package share directory: " << e.what() + << "\n"; + } + + return resourcePath; +} + +void loadWindowIcon(GLFWwindow *windowPtr, const std::filesystem::path &resourcePath) { + const auto logoPath = resourcePath / "rig_reconfigure.png"; + + std::vector iconData; + unsigned int width; + unsigned int height; + + unsigned int error = lodepng::decode(iconData, width, height, logoPath.string()); + + if (error == 0) { + GLFWimage icon; + + icon.width = static_cast(width); + icon.height = static_cast(height); + icon.pixels = iconData.data(); + + glfwSetWindowIcon(windowPtr, 1, &icon); + } else { + std::cerr << "Unable to load window icon (decoder error " << error << " - " << lodepng_error_text(error) << ")" + << std::endl; + } +} + +void glfw_error_callback(const int error, const char *description) { + fprintf(stderr, "Glfw Error %d: %s\n", error, description); +}