From b415148007b9f9a3741c4127e76fe65b551245c7 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Tue, 20 May 2025 19:20:34 +0200 Subject: [PATCH 01/12] Fix segfault in ClusterDisplay when using RViz config --- .../ClusterLabelDisplay.hpp | 10 +-- .../src/ClusterLabelDisplay.cpp | 86 ++++++++++++++----- .../src/ClusterLabelTool.cpp | 3 +- 3 files changed, 72 insertions(+), 27 deletions(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp index 59d86ba..1e03ec8 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp @@ -232,9 +232,12 @@ private Q_SLOTS: void onInitialize(); /** - * @brief Programmatically create an instance of the label tool from this package + * @brief Find the instance of the ClusterLabelTool or create one. + * + * We cannot store a pointer to the tool because RViz could sometimes replaces + * the Tool instance, and that would invalidate our pointer. */ - void initializeLabelTool(); + ClusterLabelTool* getOrCreateLabelTool(); /** * @brief Create visuals for each cluster in the list @@ -266,9 +269,6 @@ private Q_SLOTS: /// Cluster data vector m_clusterList; - /// Label tool - ClusterLabelTool* m_tool; - /// Property for the current active visual rviz_common::properties::EnumProperty* m_activeVisualProperty; diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index e838685..c9d17d1 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -92,6 +92,11 @@ Ogre::ColourValue getRainbowColor(float value) } ClusterLabelDisplay::ClusterLabelDisplay() +: m_activeVisualProperty(nullptr) +, m_alphaProperty(nullptr) +, m_colorsProperty(nullptr) +, m_sphereSizeProperty(nullptr) +, m_phantomVisualProperty(nullptr) { m_activeVisualProperty = new rviz_common::properties::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", @@ -162,7 +167,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector void ClusterLabelDisplay::onInitialize() { // Look for an existing label tool or create a new one - initializeLabelTool(); + getOrCreateLabelTool(); } void ClusterLabelDisplay::onEnable() @@ -174,7 +179,11 @@ void ClusterLabelDisplay::onDisable() { m_visuals.clear(); m_phantomVisual.reset(); - m_tool->resetVisual(); + + if (auto* tool = getOrCreateLabelTool()) + { + tool->resetVisual(); + } } // ===================================================================================================================== @@ -206,12 +215,14 @@ void ClusterLabelDisplay::updateMap() return; } - if(m_tool) + auto* tool = getOrCreateLabelTool(); + + if(tool) { // Reset the visual of the label tool so that it can be deleted - m_tool->resetVisual(); + tool->resetVisual(); } else { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Tool not initialized!"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "ClusterLabel Tool could not be initialized! It will not work!"); } // Now create the visuals for the loaded clusters @@ -230,7 +241,10 @@ void ClusterLabelDisplay::updateMap() updateColors(); // Update the tool's assigned display (to this display) - m_tool->setDisplay(this); + if (tool) + { + tool->setDisplay(this); + } // All good setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); @@ -238,7 +252,7 @@ void ClusterLabelDisplay::updateMap() void ClusterLabelDisplay::updateColors() { - for (int i = 0; i < m_colorProperties.size(); i++) + for (size_t i = 0; i < m_colorProperties.size(); i++) { auto colorProp = m_colorProperties[i]; m_visuals[i]->setColor(colorProp->getOgreColor(), m_alphaProperty->getFloat()); @@ -247,7 +261,10 @@ void ClusterLabelDisplay::updateColors() void ClusterLabelDisplay::updateSphereSize() { - m_tool->setSphereSize(m_sphereSizeProperty->getFloat()); + if (auto* tool = getOrCreateLabelTool()) + { + tool->setSphereSize(m_sphereSizeProperty->getFloat()); + } } void ClusterLabelDisplay::updatePhantomVisual() @@ -269,7 +286,7 @@ void ClusterLabelDisplay::fillPropertyOptions() m_colorsProperty->removeChildren(); m_colorProperties.clear(); - for (int i = 0; i < m_clusterList.size(); i++) + for (size_t i = 0; i < m_clusterList.size(); i++) { // Add cluster labels to dropdown menu m_activeVisualProperty->addOption(QString::fromStdString(m_clusterList[i].name), i); @@ -296,7 +313,7 @@ void ClusterLabelDisplay::createVisualsFromClusterList() // Create a visual for each entry in the cluster list float colorIndex = 0.0; // index for coloring - for (int i = 0; i < m_clusterList.size(); i++) + for (size_t i = 0; i < m_clusterList.size(); i++) { std::stringstream ss; ss << "ClusterLabelVisual_" << i; @@ -324,37 +341,66 @@ void ClusterLabelDisplay::createPhantomVisual() // ===================================================================================================================== // Label tool -void ClusterLabelDisplay::initializeLabelTool() +ClusterLabelTool* ClusterLabelDisplay::getOrCreateLabelTool() { + ClusterLabelTool* tool = nullptr; + // Check if the cluster label tool is already opened rviz_common::ToolManager* toolManager = context_->getToolManager(); QStringList toolClasses = toolManager->getToolClasses(); - bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) { if (toolClasses[i].contains("ClusterLabel")) { - m_tool = static_cast(toolManager->getTool(i)); - foundTool = true; - break; + tool = dynamic_cast(toolManager->getTool(i)); + if (nullptr == tool) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found Tool with 'ClusterLabel' class but could not dynamic_cast to ClusterLabelTool*"); + return nullptr; + } } } - if (!foundTool) + // Create a Tool instance + if (!tool) { auto tool_tmp = context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel"); - if(m_tool = dynamic_cast(tool_tmp); m_tool != nullptr) + if(tool = dynamic_cast(tool_tmp); tool != nullptr) + { + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Created ClusterLabelTool at Address: %llx", (unsigned long long) tool); + } + else { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Created ClusterLabelTool"); - } else { RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not create ClusterLabelTool"); + return nullptr; } } + + // Set all settings the ClusterLabelTool needs to know + tool->setSphereSize(m_sphereSizeProperty->getFloat()); + if (!m_visuals.empty()) + { + tool->setVisual(m_visuals[m_activeVisualId]); + } + + // The Tool will request the geometry and segfault (also we will log an error in getGeometry) + if (m_geometry) + { + tool->setDisplay(this); + } + + return tool; } void ClusterLabelDisplay::notifyLabelTool() { - m_tool->setVisual(m_visuals[m_activeVisualId]); + auto* tool = getOrCreateLabelTool(); + if (!tool) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed to notify LabelTool of active Visual Change!"); + return; + } + tool->setVisual(m_visuals[m_activeVisualId]); } void ClusterLabelDisplay::addLabel(std::string label, std::vector faces) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 7a7fe98..46bcb3c 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -893,7 +893,6 @@ void ClusterLabelTool::resetFaces() void ClusterLabelTool::resetVisual() { - // TODO: Segfault here, when using RViz config m_visual.reset(); } @@ -902,4 +901,4 @@ void ClusterLabelTool::resetVisual() #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelTool, rviz_common::Tool) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelTool, rviz_common::Tool) From d81c0cd84c3a2fba6bb7c2954afbf1ca6da0fd0a Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Wed, 21 May 2025 16:20:20 +0200 Subject: [PATCH 02/12] Reenable saving and loading of labeled clusters. --- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 57 ++++++++++++++++++++-- 1 file changed, 52 insertions(+), 5 deletions(-) diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index b8dd527..45a8c56 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -546,6 +546,46 @@ bool MapDisplay::loadData() m_costs[elem.first] = copy_1d_channel_to_vector(elem.second.extract()); } } + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load labeled clusters..."); + m_clusterList.clear(); + + auto& hdf5_file = hdf5_mesh_io->m_hdf5_file; + const std::string labels_group_name = mesh_part + "/labels"; + if (hdf5_file->exist(labels_group_name)) + { + auto labels_group = hdf5_file->getGroup(mesh_part + "/labels"); + for (const auto& gname: labels_group.listObjectNames()) + { + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Loading label: %s", gname.c_str()); + // Load this labels instances + auto group = labels_group.getGroup(gname); + for (const auto& iname: group.listObjectNames()) + { + // Check if we have a dataset + if (HighFive::ObjectType::Dataset != group.getObjectType(iname)) + { + continue; + } + + // Load the dataset + size_t n = 0; + const auto cluster_array = hdf5_mesh_io->loadArray(group.getPath(), iname, n); + if (0 == n) + { + continue; + } + + // Convert to vector + Cluster instance("", {}); + // Instance 5 of label door = door_5 + instance.name = gname + "_" + iname; + instance.faces.resize(n); + std::copy(cluster_array.get(), cluster_array.get() + n,instance.faces.begin()); + m_clusterList.push_back(std::move(instance)); + } + } + } } else { @@ -788,12 +828,19 @@ void MapDisplay::saveLabel(Cluster cluster) return; } - // is not working anyway - // // Open IO - // hdf5_map_io::HDF5MapIO map_io(m_mapFilePath->getFilename()); + // Open IO + auto hdf5_mesh_io = std::make_shared(); + hdf5_mesh_io->open(m_mapFilePath->getFilename()); + + // Build HDF Group name + const std::string mesh_part = "mesh"; + const std::string label_group = mesh_part + "/labels/" + results[0]; - // // Add label with faces list - // map_io.addOrUpdateLabel(results[0], results[1], faces); + // Use Array IO to save each instance as a dataset + // Array IO needs shared array + auto faces_array = lvr2::indexArray(new unsigned int[faces.size()]); + std::copy(faces.begin(), faces.end(), faces_array.get()); + hdf5_mesh_io->ArrayIO::save(label_group, results[1], faces.size(), faces_array); // Add to cluster list m_clusterList.push_back(Cluster(label, faces)); From 05b4e0bd14276345a0c328910f9d097ce6da61ee Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Thu, 22 May 2025 13:59:09 +0200 Subject: [PATCH 03/12] Accelerated area picking via offscreen rendering pass --- .../ClusterLabelTool.hpp | 24 +- .../src/ClusterLabelTool.cpp | 403 ++++++++---------- .../src/ClusterLabelVisual.cpp | 1 + rviz_mesh_tools_plugins/src/MeshVisual.cpp | 5 + 4 files changed, 191 insertions(+), 242 deletions(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 9741b00..7aab25d 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -261,23 +261,13 @@ public Q_SLOTS: std::vector m_boxData; std::vector m_resultDistances; - // OpenCL - // cl::Device m_clDevice; - // cl::Context m_clContext; - // cl::Program::Sources m_clProgramSources; - // cl::Program m_clProgram; - // cl::CommandQueue m_clQueue; - // cl::Buffer m_clVertexBuffer; - // cl::Buffer m_clResultBuffer; - // cl::Buffer m_clRayBuffer; - // cl::Buffer m_clSphereBuffer; - // cl::Buffer m_clBoxBuffer; - // cl::Buffer m_clStartNormalBuffer; - // cl::Kernel m_clKernelSingleRay; - // cl::Kernel m_clKernelSphere; - // cl::Kernel m_clKernelBox; - // cl::Kernel m_clKernelDirAndDist; + // Accelerated area picking via Ogre render pass + Ogre::TexturePtr m_selectionTexture; + Ogre::MaterialPtr m_selectionMaterial; + Ogre::ManualObject* m_selectionMesh; + Ogre::SceneNode* m_selectionSceneNode; + uint32_t m_selectionVisibilityBit; }; } // end namespace rviz_mesh_tools_plugins -#endif \ No newline at end of file +#endif diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 46bcb3c..3a6a1a3 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -45,6 +45,7 @@ * */ +#include #include #include #include @@ -61,10 +62,9 @@ #include #include - #include - #include +#include #include @@ -72,6 +72,10 @@ #include #include #include +#include +#include +#include +#include #include @@ -91,6 +95,7 @@ ClusterLabelTool::ClusterLabelTool() ,m_displayInitialized(false) // ,m_evt_start(nullptr, (QMouseEvent*)nullptr, 0, 0) // ,m_evt_stop(nullptr, (QMouseEvent*)nullptr, 0, 0) +, m_selectionMesh(nullptr) { shortcut_key_ = 'l'; } @@ -101,6 +106,8 @@ ClusterLabelTool::~ClusterLabelTool() context_->getSceneManager()->destroyManualObject(m_selectionBox->getName()); context_->getSceneManager()->destroyManualObject(m_selectionBoxMaterial->getName()); context_->getSceneManager()->destroySceneNode(m_sceneNode); + context_->getSceneManager()->destroySceneNode(m_selectionSceneNode); + context_->getSceneManager()->destroyManualObject(m_selectionMesh); } // onInitialize() is called by the superclass after scene_manager_ and @@ -138,131 +145,43 @@ void ClusterLabelTool::onInitialize() m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); + m_selectionVisibilityBit = context_->visibilityBits()->allocBit(); - // // try-catch block to check for OpenCL errors - // try - // { - // // Initialize OpenCL - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Get platforms"); - // vector platforms; - // cl::Platform::get(&platforms); - // for (auto const& platform : platforms) - // { - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found platform: %s", platform.getInfo().c_str()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "platform version: %s", platform.getInfo().c_str()); - // } - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); - - // vector consideredDevices; - // for (auto const& platform : platforms) - // { - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Get devices of %s: ", platform.getInfo().c_str()); - // cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; - // m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); - // vector devices = m_clContext.getInfo(); - // for (auto const& device : devices) - // { - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found device: %s", device.getInfo().c_str()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Device work units: %d", device.getInfo()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Device work group size: %lu", device.getInfo()); - - // std::string device_info = device.getInfo(); - // // getVersion extracts the version number with major in the upper 16 bits and minor in the lower 16 bits - - // unsigned int version = cl::detail::getVersion(std::vector(device_info.begin(), device_info.end())); - - // // shift 16 to the right to get the number in the upper 16 bits - // cl_uint majorVersion = version >> 16; - // // use bitwise AND to extract the number in the lower 16 bits - // cl_uint minorVersion = version & 0x0000FFFF; - - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found a device with OpenCL version: %u.%u", majorVersion, minorVersion); - - // // find all devices that support at least OpenCL version 1.2 - // if (majorVersion >= 1 && minorVersion >= 2) - // ; - // { - // consideredDevices.push_back(device); - // } - // } - // } - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); - - // cl::Platform platform; - // // Preferably choose the first compatible device of type GPU - // bool deviceFound = false; - // for (auto const& device : consideredDevices) - // { - // if (device.getInfo() == CL_DEVICE_TYPE_GPU) - // { - // m_clDevice = device; - // platform = device.getInfo(); - // deviceFound = true; - // break; - // } - // } - // if (!deviceFound && consideredDevices.size() > 0) - // { - // // If no device of type GPU was found, choose the first compatible device - // m_clDevice = consideredDevices[0]; - // platform = m_clDevice.getInfo(); - // deviceFound = true; - // } - // if (!deviceFound) - // { - // // Panic if no compatible device was found - // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "No device with compatible OpenCL version found (minimum 2.0)"); - // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - // rclcpp::shutdown(); - // } - - // cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; - // m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); - - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Using device %s of platform %s", m_clDevice.getInfo().c_str(), - // platform.getInfo().c_str()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); - - // // Read kernel file - // // may throw ament_index_cpp::PackageNotFoundError exception - // std::string package_share_directory = ament_index_cpp::get_package_share_directory("my_package_name"); - - // ifstream in(package_share_directory + CL_RAY_CAST_KERNEL_FILE); - // std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - - - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Got kernel: %s%s", package_share_directory.c_str(), CL_RAY_CAST_KERNEL_FILE); - - // m_clProgramSources = cl::Program::Sources(1, { cast_rays_kernel.c_str(), cast_rays_kernel.length() }); - - // m_clProgram = cl::Program(m_clContext, m_clProgramSources); - // try - // { - // m_clProgram.build({ m_clDevice }); - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successfully built program."); - // } - // catch (cl::Error& err) - // { - - // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); - // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - // rclcpp::shutdown(); - // // ros::shutdown(); - // exit(1); - // } - - // // Create queue to which we will push commands for the device. - // m_clQueue = cl::CommandQueue(m_clContext, m_clDevice, 0); - // } - // catch (cl::Error err) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); - // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - // rclcpp::shutdown(); - // // ros::requestShutdown(); - // exit(1); - // } + // Setup texture for accelerated picking + // We render the scene in a 1280x720 resolution + m_selectionTexture = Ogre::TextureManager::getSingleton().getByName("ClusterLabelToolPickRenderTex"); + if (nullptr == m_selectionTexture) + { + m_selectionTexture = Ogre::TextureManager::getSingleton().createManual( + "ClusterLabelToolPickRenderTex", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, + 1280, 720, + 0, + Ogre::PF_R8G8B8, + Ogre::TU_RENDERTARGET + ); + } + + m_selectionMaterial = Ogre::MaterialManager::getSingleton().getByName("ClusterLabelToolSelectionMat"); + if (nullptr == m_selectionMaterial) + { + m_selectionMaterial = Ogre::MaterialManager::getSingleton().create( + "ClusterLabelToolSelectionMat", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true + ); + } + + Ogre::Technique* tech = m_selectionMaterial->getTechnique(0); + Ogre::Pass* pass = tech->getPass(0); + pass->setLightingEnabled(false); + pass->setAmbient(1, 1, 1); + + // Scene Node for our modified mesh + m_selectionSceneNode = scene_manager_->getSceneNode("ClusterLabelToolSelection", false); + if (nullptr == m_selectionSceneNode) + { + m_selectionSceneNode = scene_manager_->getRootSceneNode()->createChildSceneNode("ClusterLabelToolSelection"); + } } void ClusterLabelTool::setVisual(std::shared_ptr visual) @@ -319,49 +238,68 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) } } - // try-catch block to check for OpenCL errors - // try - // { - // m_clVertexBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY | CL_MEM_COPY_HOST_PTR, - // sizeof(float) * m_vertexData.size(), m_vertexData.data()); - - // m_clResultBuffer = cl::Buffer(m_clContext, CL_MEM_WRITE_ONLY | CL_MEM_HOST_READ_ONLY, - // sizeof(float) * m_meshGeometry->faces.size()); - - // m_clRayBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 6); - - // m_clSphereBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 4); - - // m_clBoxBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 4 * 6); - - // m_clStartNormalBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 3); + // For GPU accelerated picking we need per face attributes in the shader. + // Sadly Ogre 1.12 does not have a way to do this. + // Solution: Create a renderable geometry with 3 vertices per face and + // add a vertex property with the original face_id + // Then we can write a shader to write the face id to rendered pixels. + if (nullptr == m_selectionMesh) + { + m_selectionMesh = scene_manager_->createManualObject("ClusterLabelToolPickingMesh"); + m_selectionMesh->setDynamic(false); + m_selectionMesh->begin(m_selectionBoxMaterial, Ogre::RenderOperation::OT_TRIANGLE_LIST); + } + else + { + m_selectionMesh->beginUpdate(0); + } - // m_clKernelSingleRay = cl::Kernel(m_clProgram, "cast_rays"); - // m_clKernelSphere = cl::Kernel(m_clProgram, "cast_sphere"); - // m_clKernelBox = cl::Kernel(m_clProgram, "cast_box"); + // We can only use RGB (24) bits to to represent triangle id + const uint32_t MAX_NUM_FACES = std::pow(2, 24); + if (MAX_NUM_FACES > m_meshGeometry->faces.size()) + { + RCLCPP_WARN( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "ClusterLabelTool: Mesh to label has more than the supported number of triangles (%u). This can lead to faces not being selected!", + MAX_NUM_FACES + ); + } - // m_clKernelSingleRay.setArg(0, m_clVertexBuffer); - // m_clKernelSingleRay.setArg(1, m_clRayBuffer); - // m_clKernelSingleRay.setArg(2, m_clResultBuffer); + for (uint32_t faceID = 0; faceID < m_meshGeometry->faces.size(); faceID++) + { + const auto& face = m_meshGeometry->faces[faceID]; + for (int i = 0; i < 3; i++) + { + const auto& v = m_meshGeometry->vertices[face.vertexIndices[i]]; + m_selectionMesh->position(v.x, v.y, v.z); + Ogre::ColourValue color; + color.setAsARGB(faceID); + color.a = 1.0; + m_selectionMesh->colour(color); + } + m_selectionMesh->triangle(faceID * 3 + 0, faceID * 3 + 1, faceID * 3 + 2); + } + m_selectionMesh->end(); + m_selectionMesh->setVisibilityFlags(m_selectionVisibilityBit); + m_selectionMesh->setMaterial(0, m_selectionMaterial); - // m_clKernelSphere.setArg(0, m_clVertexBuffer); - // m_clKernelSphere.setArg(1, m_clSphereBuffer); - // m_clKernelSphere.setArg(2, m_clResultBuffer); - // m_clKernelSphere.setArg(3, m_sphereSize); + if (auto* parent = m_selectionSceneNode->getParentSceneNode()) + { + parent->removeChild(m_selectionSceneNode); + } - // m_clKernelBox.setArg(0, m_clVertexBuffer); - // m_clKernelBox.setArg(1, m_clBoxBuffer); - // m_clKernelBox.setArg(2, m_clResultBuffer); - // } - // catch (cl::Error err) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); - // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - // rclcpp::shutdown(); - // // ros::shutdown(); - // exit(1); - // } + const auto& c = display->getSceneNode()->getChildren(); + if (c.end() == std::find(c.begin(), c.end(), m_selectionSceneNode)) + { + display->getSceneNode()->addChild(m_selectionSceneNode); + } + m_selectionSceneNode->detachObject(m_selectionMesh); + RCLCPP_INFO( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "ClusterLabelTool: Load Mesh with %lu faces", m_meshGeometry->faces.size() + ); + m_faceSelectedArray.clear(); + m_faceSelectedArray.resize(m_meshGeometry->faces.size()); } void ClusterLabelTool::updateSelectionBox() @@ -428,7 +366,6 @@ void ClusterLabelTool::selectionBoxMove(rviz_common::ViewportMouseEvent& event) // Not possible // m_selectionStop.x = (float)event.x / event.panel->getRenderWindow()->width(); // m_selectionStop.y = (float)event.y / event.panel->getRenderWindow()->height(); - m_bb_x2 = event.x; m_bb_y2 = event.y; @@ -443,78 +380,94 @@ void ClusterLabelTool::selectMultipleFaces( { m_selectionBox->setVisible(false); - int left = m_bb_x1; - int right = m_bb_x2; - int top = m_bb_y1; - int bottom = m_bb_y2; + // Attach the mesh to scene node + m_selectionSceneNode->attachObject(m_selectionMesh); - size_t goalSection; - size_t goalIndex; + // Get the current rviz camera + Ogre::Camera* camera = context_->getViewManager()->getCurrent()->getCamera(); - if (left > right) - { - std::swap(left, right); - } + // Setup the render target to be the texture + Ogre::RenderTexture* render_texture = m_selectionTexture->getBuffer()->getRenderTarget(); + Ogre::Viewport* viewport = render_texture->addViewport(camera); + viewport->setClearEveryFrame(true); + viewport->setOverlaysEnabled(false); + viewport->setBackgroundColour(Ogre::ColourValue::White); + viewport->setVisibilityMask(m_selectionVisibilityBit); - if (top > bottom) - { - std::swap(top, bottom); - } - - const int BOX_SIZE_TOLERANCE = 1; - if ((right - left) * (bottom - top) < BOX_SIZE_TOLERANCE) - { - // single face - selectSingleFace(event, selectMode); - } else { - // std::cout << "PICK!" << std::endl; - rviz_common::interaction::M_Picked pick_results; + // Actually render offscreen + render_texture->update(); - auto manager = context_->getSelectionManager(); + // Copy the data to ram + // TODO: We could copy only the rect we are interested in? + Ogre::Image img(Ogre::PF_R8G8B8, render_texture->getWidth(), render_texture->getHeight()); + Ogre::PixelBox pixels = img.getPixelBox(); - manager->pick( - m_evt_panel->getRenderWindow(), - m_bb_x1, m_bb_y1, - m_bb_x2, m_bb_y2, - pick_results); + render_texture->copyContentsToMemory(pixels, pixels); - if(!pick_results.empty()) + // At this point we need to inspect the contents + + // Translate to uv coordinates + uint32_t xl = std::min(m_bb_x1, m_bb_x2); + uint32_t xr = std::max(m_bb_x1, m_bb_x2); + uint32_t yl = std::min(m_bb_y1, m_bb_y2); + uint32_t yr = std::max(m_bb_y1, m_bb_y2); + + const float u_min = std::clamp((float) xl / m_evt_panel->width(), 0.0f, 1.0f); + const float u_max = std::clamp((float) xr / m_evt_panel->width(), 0.0f, 1.0f); + const float v_min = std::clamp((float) yl / m_evt_panel->height(), 0.0f, 1.0f); + const float v_max = std::clamp((float) yr / m_evt_panel->height(), 0.0f, 1.0f); + + RCLCPP_DEBUG( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "Selected rect uv: (%f, %f) (%f, %f)", + u_min, v_min, u_max, v_max + ); + + const uint32_t tex_x0 = u_min * m_selectionTexture->getWidth(); + const uint32_t tex_x1 = u_max * m_selectionTexture->getWidth(); + const uint32_t tex_y0 = v_min * m_selectionTexture->getHeight(); + const uint32_t tex_y1 = v_max * m_selectionTexture->getHeight(); + + RCLCPP_DEBUG( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "Selected rect tex: (%u, %u) (%u, %u)", + tex_x0, tex_y0, tex_x1, tex_y1 + ); + + img.save("test.png"); + // Update all faces in the selected region + for (uint32_t y = tex_y0; y < tex_y1; y++) + { + for (uint32_t x = tex_x0; x < tex_x1; x++) { - // FOUND SOMETHING! - // std::cout << "FOUND SOMETHING! " << pick_results.size() << std::endl; - - for(auto elem : pick_results) + Ogre::ColourValue color = img.getColourAt(x, y, 0); + color.a = 0.0; + uint32_t face_id = color.getAsARGB(); + if (face_id >= m_faceSelectedArray.size()) { - rviz_common::interaction::CollObjectHandle key = elem.first; - rviz_common::interaction::Picked value = elem.second; - - // std::cout << key << std::endl; - // std::cout << "Extra handles: " << value.extra_handles.size() << std::endl; - + // These are pixels which have the background color + continue; + } + else + { + m_faceSelectedArray[face_id] = selectMode; } - - // TODO: continue implementing this - } else { - // std::cout << "NOTHING to pick :(" << std::endl; } } + m_selectionSceneNode->detachAllObjects(); + render_texture->removeAllViewports(); - // Ogre::PlaneBoundedVolume volume = - // event.panel->getViewController()->getCamera()->getCameraToViewportBoxVolume(left, top, right, bottom, true); - - // selectFacesInBoxParallel(volume, selectMode); - - // auto manager = context_->getSelectionManager(); - - // virtual void SelectionManager::pick( - // rviz_rendering::RenderWindow * window, - // int x1, - // int y1, - // int x2, - // int y2, - // M_Picked & results) = 0; - + // Update the visual with the selected faces + std::vector tmpFaceList; + for(size_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + tmpFaceList.push_back(faceId); + } + } + m_visual->setFacesInCluster(tmpFaceList); } void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index 2b5c17f..ddf95eb 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -147,6 +147,7 @@ ClusterLabelVisual::ClusterLabelVisual( RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "nullptr return: sceneManager->createEntity(\"ClusterLabelEntity\", \"ClusterLabelMesh\", \"General\"); "); } entity->setMaterialName("CustomMaterial", "General"); + entity->setVisibilityFlags(m_displayContext->getDefaultVisibilityBit()); m_sceneNode->attachObject(entity); } diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp index 7b0860e..0c76ed2 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -133,30 +133,35 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random; m_mesh = sceneManager->createManualObject(sstm.str()); m_mesh->setDynamic(false); + m_mesh->setVisibilityFlags(context->getDefaultVisibilityBit()); m_sceneNode->attachObject(m_mesh); std::stringstream sstmNormals; sstmNormals << m_prefix << "_Normals_" << m_postfix << "_" << m_random; m_normals = sceneManager->createManualObject(sstmNormals.str()); m_normals->setDynamic(false); + m_normals->setVisibilityFlags(context->getDefaultVisibilityBit()); m_sceneNode->attachObject(m_normals); std::stringstream sstmTexturedMesh; sstmTexturedMesh << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random; m_texturedMesh = sceneManager->createManualObject(sstmTexturedMesh.str()); m_texturedMesh->setDynamic(false); + m_texturedMesh->setVisibilityFlags(context->getDefaultVisibilityBit()); m_sceneNode->attachObject(m_texturedMesh); std::stringstream sstmNoTexCluMesh; sstmNoTexCluMesh << m_prefix << "_NoTexCluMesh_" << m_postfix << "_" << m_random; m_noTexCluMesh = sceneManager->createManualObject(sstmNoTexCluMesh.str()); m_noTexCluMesh->setDynamic(false); + m_noTexCluMesh->setVisibilityFlags(context->getDefaultVisibilityBit()); m_sceneNode->attachObject(m_noTexCluMesh); std::stringstream sstmVertexCostsMesh; sstmVertexCostsMesh << m_prefix << "_VertexCostsMesh_" << m_postfix << "_" << m_random; m_vertexCostsMesh = sceneManager->createManualObject(sstmVertexCostsMesh.str()); m_vertexCostsMesh->setDynamic(false); + m_vertexCostsMesh->setVisibilityFlags(context->getDefaultVisibilityBit()); m_sceneNode->attachObject(m_vertexCostsMesh); } From 67851d0a643e1eddfbea8aa6e85c1906e0af0665 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Fri, 23 May 2025 12:01:51 +0200 Subject: [PATCH 04/12] Move Offscreen rendering to seperate method for later reuse --- .../ClusterLabelTool.hpp | 18 +- .../src/ClusterLabelDisplay.cpp | 13 +- .../src/ClusterLabelTool.cpp | 537 ++++++------------ 3 files changed, 176 insertions(+), 392 deletions(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 7aab25d..4366826 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -245,12 +245,8 @@ public Q_SLOTS: void selectionBoxStart(rviz_common::ViewportMouseEvent& event); void selectionBoxMove(rviz_common::ViewportMouseEvent& event); void selectMultipleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); - void selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode); void selectSingleFace(rviz_common::ViewportMouseEvent& event, bool selectMode); - void selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode); void selectSphereFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); - void selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode); - boost::optional> getClosestIntersectedFaceParallel(Ogre::Ray& ray); rclcpp::Publisher::SharedPtr m_labelPublisher; @@ -261,6 +257,20 @@ public Q_SLOTS: std::vector m_boxData; std::vector m_resultDistances; + /** + * @brief Renders the current Mesh to an Offscreen Buffer using the FaceIDs as colors. + * + * The resulting Image can be used to determine which faces are visible from the Camera. + * + * @return The rendered Image. + */ + Ogre::Image renderMeshWithFaceID(); + + /** + * @brief Setup the Selection Mesh from the current geometry + */ + void updateSelectionMesh(); + // Accelerated area picking via Ogre render pass Ogre::TexturePtr m_selectionTexture; Ogre::MaterialPtr m_selectionMaterial; diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index c9d17d1..40cc09c 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -374,20 +374,11 @@ ClusterLabelTool* ClusterLabelDisplay::getOrCreateLabelTool() RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not create ClusterLabelTool"); return nullptr; } - } - // Set all settings the ClusterLabelTool needs to know - tool->setSphereSize(m_sphereSizeProperty->getFloat()); - if (!m_visuals.empty()) - { - tool->setVisual(m_visuals[m_activeVisualId]); + // Set all settings the ClusterLabelTool needs to know + tool->setSphereSize(m_sphereSizeProperty->getFloat()); } - // The Tool will request the geometry and segfault (also we will log an error in getGeometry) - if (m_geometry) - { - tool->setDisplay(this); - } return tool; } diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 3a6a1a3..9dbf292 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -108,6 +108,7 @@ ClusterLabelTool::~ClusterLabelTool() context_->getSceneManager()->destroySceneNode(m_sceneNode); context_->getSceneManager()->destroySceneNode(m_selectionSceneNode); context_->getSceneManager()->destroyManualObject(m_selectionMesh); + context_->visibilityBits()->freeBits(m_selectionVisibilityBit); } // onInitialize() is called by the superclass after scene_manager_ and @@ -149,6 +150,7 @@ void ClusterLabelTool::onInitialize() // Setup texture for accelerated picking // We render the scene in a 1280x720 resolution + // There should only ever be one instance of this Tool so we can reuse the texture m_selectionTexture = Ogre::TextureManager::getSingleton().getByName("ClusterLabelToolPickRenderTex"); if (nullptr == m_selectionTexture) { @@ -163,6 +165,7 @@ void ClusterLabelTool::onInitialize() ); } + // Create a material m_selectionMaterial = Ogre::MaterialManager::getSingleton().getByName("ClusterLabelToolSelectionMat"); if (nullptr == m_selectionMaterial) { @@ -189,12 +192,16 @@ void ClusterLabelTool::setVisual(std::shared_ptr visual) // set new visual m_visual = visual; m_selectedFaces = visual->getFaces(); - m_faceSelectedArray.clear(); + + // Reset the selected faces + std::fill(m_faceSelectedArray.begin(), m_faceSelectedArray.end(), false); for (auto faceId : m_selectedFaces) { if (m_faceSelectedArray.size() <= faceId) { - m_faceSelectedArray.resize(faceId + 1); + // The faceSelectedArray has space for all faces + // This is an error from the Visual + continue; } m_faceSelectedArray[faceId] = true; } @@ -218,7 +225,8 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) { m_display = display; m_meshGeometry = m_display->getGeometry(); - m_faceSelectedArray.reserve(m_meshGeometry->faces.size()); + m_faceSelectedArray.resize(m_meshGeometry->faces.size()); + std::fill(m_faceSelectedArray.begin(), m_faceSelectedArray.end(), false); m_displayInitialized = true; m_vertexData.reserve(m_meshGeometry->faces.size() * 3 * 3); @@ -238,68 +246,8 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) } } - // For GPU accelerated picking we need per face attributes in the shader. - // Sadly Ogre 1.12 does not have a way to do this. - // Solution: Create a renderable geometry with 3 vertices per face and - // add a vertex property with the original face_id - // Then we can write a shader to write the face id to rendered pixels. - if (nullptr == m_selectionMesh) - { - m_selectionMesh = scene_manager_->createManualObject("ClusterLabelToolPickingMesh"); - m_selectionMesh->setDynamic(false); - m_selectionMesh->begin(m_selectionBoxMaterial, Ogre::RenderOperation::OT_TRIANGLE_LIST); - } - else - { - m_selectionMesh->beginUpdate(0); - } - - // We can only use RGB (24) bits to to represent triangle id - const uint32_t MAX_NUM_FACES = std::pow(2, 24); - if (MAX_NUM_FACES > m_meshGeometry->faces.size()) - { - RCLCPP_WARN( - rclcpp::get_logger("rviz_mesh_tools_plugins"), - "ClusterLabelTool: Mesh to label has more than the supported number of triangles (%u). This can lead to faces not being selected!", - MAX_NUM_FACES - ); - } - - for (uint32_t faceID = 0; faceID < m_meshGeometry->faces.size(); faceID++) - { - const auto& face = m_meshGeometry->faces[faceID]; - for (int i = 0; i < 3; i++) - { - const auto& v = m_meshGeometry->vertices[face.vertexIndices[i]]; - m_selectionMesh->position(v.x, v.y, v.z); - Ogre::ColourValue color; - color.setAsARGB(faceID); - color.a = 1.0; - m_selectionMesh->colour(color); - } - m_selectionMesh->triangle(faceID * 3 + 0, faceID * 3 + 1, faceID * 3 + 2); - } - m_selectionMesh->end(); - m_selectionMesh->setVisibilityFlags(m_selectionVisibilityBit); - m_selectionMesh->setMaterial(0, m_selectionMaterial); - - if (auto* parent = m_selectionSceneNode->getParentSceneNode()) - { - parent->removeChild(m_selectionSceneNode); - } - - const auto& c = display->getSceneNode()->getChildren(); - if (c.end() == std::find(c.begin(), c.end(), m_selectionSceneNode)) - { - display->getSceneNode()->addChild(m_selectionSceneNode); - } - m_selectionSceneNode->detachObject(m_selectionMesh); - RCLCPP_INFO( - rclcpp::get_logger("rviz_mesh_tools_plugins"), - "ClusterLabelTool: Load Mesh with %lu faces", m_meshGeometry->faces.size() - ); - m_faceSelectedArray.clear(); - m_faceSelectedArray.resize(m_meshGeometry->faces.size()); + // Prepare for GPU accelerated selection + updateSelectionMesh(); } void ClusterLabelTool::updateSelectionBox() @@ -340,14 +288,6 @@ void ClusterLabelTool::updateSelectionBox() void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) { - // OLD: doesnt work anymore - // m_selectionStart.x = (float)event.x / event.viewport->getActualWidth(); - // m_selectionStart.y = (float)event.y / event.viewport->getActualHeight(); - // NEW: - // TODO: Check if this is right - // m_selectionStart.x = (float)event.x / event.panel->getRenderWindow()->width(); - // m_selectionStart.y = (float)event.y / event.panel->getRenderWindow()->height(); - m_bb_x1 = event.x; m_bb_y1 = event.y; m_evt_panel = event.panel; @@ -360,13 +300,6 @@ void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) void ClusterLabelTool::selectionBoxMove(rviz_common::ViewportMouseEvent& event) { - // m_selectionStop.x = (float)event.x / event.viewport->getActualWidth(); - // m_selectionStop.y = (float)event.y / event.viewport->getActualHeight(); - - // Not possible - // m_selectionStop.x = (float)event.x / event.panel->getRenderWindow()->width(); - // m_selectionStop.y = (float)event.y / event.panel->getRenderWindow()->height(); - m_bb_x2 = event.x; m_bb_y2 = event.y; m_evt_panel = event.panel; @@ -378,44 +311,22 @@ void ClusterLabelTool::selectMultipleFaces( rviz_common::ViewportMouseEvent& event, bool selectMode) { + (void) event; m_selectionBox->setVisible(false); - // Attach the mesh to scene node - m_selectionSceneNode->attachObject(m_selectionMesh); - - // Get the current rviz camera - Ogre::Camera* camera = context_->getViewManager()->getCurrent()->getCamera(); - - // Setup the render target to be the texture - Ogre::RenderTexture* render_texture = m_selectionTexture->getBuffer()->getRenderTarget(); - Ogre::Viewport* viewport = render_texture->addViewport(camera); - viewport->setClearEveryFrame(true); - viewport->setOverlaysEnabled(false); - viewport->setBackgroundColour(Ogre::ColourValue::White); - viewport->setVisibilityMask(m_selectionVisibilityBit); - - // Actually render offscreen - render_texture->update(); - - // Copy the data to ram - // TODO: We could copy only the rect we are interested in? - Ogre::Image img(Ogre::PF_R8G8B8, render_texture->getWidth(), render_texture->getHeight()); - Ogre::PixelBox pixels = img.getPixelBox(); + Ogre::Image img = renderMeshWithFaceID(); + img.save("test.png"); - render_texture->copyContentsToMemory(pixels, pixels); + // Translate the selection box to uv coordinates + uint32_t x0 = std::min(m_bb_x1, m_bb_x2); + uint32_t x1 = std::max(m_bb_x1, m_bb_x2); + uint32_t y0 = std::min(m_bb_y1, m_bb_y2); + uint32_t y1 = std::max(m_bb_y1, m_bb_y2); - // At this point we need to inspect the contents - - // Translate to uv coordinates - uint32_t xl = std::min(m_bb_x1, m_bb_x2); - uint32_t xr = std::max(m_bb_x1, m_bb_x2); - uint32_t yl = std::min(m_bb_y1, m_bb_y2); - uint32_t yr = std::max(m_bb_y1, m_bb_y2); - - const float u_min = std::clamp((float) xl / m_evt_panel->width(), 0.0f, 1.0f); - const float u_max = std::clamp((float) xr / m_evt_panel->width(), 0.0f, 1.0f); - const float v_min = std::clamp((float) yl / m_evt_panel->height(), 0.0f, 1.0f); - const float v_max = std::clamp((float) yr / m_evt_panel->height(), 0.0f, 1.0f); + const float u_min = std::clamp((float) x0 / m_evt_panel->width(), 0.0f, 1.0f); + const float u_max = std::clamp((float) x1 / m_evt_panel->width(), 0.0f, 1.0f); + const float v_min = std::clamp((float) y0 / m_evt_panel->height(), 0.0f, 1.0f); + const float v_max = std::clamp((float) y1 / m_evt_panel->height(), 0.0f, 1.0f); RCLCPP_DEBUG( rclcpp::get_logger("rviz_mesh_tools_plugins"), @@ -423,10 +334,11 @@ void ClusterLabelTool::selectMultipleFaces( u_min, v_min, u_max, v_max ); - const uint32_t tex_x0 = u_min * m_selectionTexture->getWidth(); - const uint32_t tex_x1 = u_max * m_selectionTexture->getWidth(); - const uint32_t tex_y0 = v_min * m_selectionTexture->getHeight(); - const uint32_t tex_y1 = v_max * m_selectionTexture->getHeight(); + // Translate to image coordinates + const uint32_t tex_x0 = u_min * img.getWidth(); + const uint32_t tex_x1 = u_max * img.getWidth(); + const uint32_t tex_y0 = v_min * img.getHeight(); + const uint32_t tex_y1 = v_max * img.getHeight(); RCLCPP_DEBUG( rclcpp::get_logger("rviz_mesh_tools_plugins"), @@ -434,7 +346,6 @@ void ClusterLabelTool::selectMultipleFaces( tex_x0, tex_y0, tex_x1, tex_y1 ); - img.save("test.png"); // Update all faces in the selected region for (uint32_t y = tex_y0; y < tex_y1; y++) { @@ -454,10 +365,10 @@ void ClusterLabelTool::selectMultipleFaces( } } } - m_selectionSceneNode->detachAllObjects(); - render_texture->removeAllViewports(); // Update the visual with the selected faces + // TODO: How to do this without creating the tmpFaceList all the time + // Maybe use a std::set? std::vector tmpFaceList; for(size_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) { @@ -470,63 +381,6 @@ void ClusterLabelTool::selectMultipleFaces( m_visual->setFacesInCluster(tmpFaceList); } -void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode) -{ - m_boxData.clear(); - for (Ogre::Plane plane : volume.planes) - { - m_boxData.push_back(plane.normal.x); - m_boxData.push_back(plane.normal.y); - m_boxData.push_back(plane.normal.z); - m_boxData.push_back(plane.d); - } - - // try - // { - // m_clQueue.enqueueWriteBuffer(m_clBoxBuffer, CL_TRUE, 0, sizeof(float) * 4 * 6, m_boxData.data()); - - // m_clQueue.enqueueNDRangeKernel(m_clKernelBox, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - // cl::NullRange, nullptr); - // m_clQueue.finish(); - - // m_resultDistances.resize(m_meshGeometry->faces.size()); - // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - // m_resultDistances.data()); - // } - // catch (cl::Error err) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); - // } - - for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) - { - if (m_resultDistances[faceId] > 0) - { - if (m_faceSelectedArray.size() <= faceId) - { - m_faceSelectedArray.resize(faceId + 1); - } - m_faceSelectedArray[faceId] = selectMode; - } - } - - std::vector tmpFaceList; - - for (uint32_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } - } - - if (m_displayInitialized && m_visual) - { - m_visual->setFacesInCluster(tmpFaceList); - // m_visual->setColor(Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f)); - } -} void ClusterLabelTool::selectSingleFace( rviz_common::ViewportMouseEvent& event, @@ -551,7 +405,7 @@ void ClusterLabelTool::selectSingleFace( } m_faceSelectedArray[intersection.face_id] = selectMode; - for(int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + for(size_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) { if (m_faceSelectedArray[faceId]) { @@ -568,185 +422,13 @@ void ClusterLabelTool::selectSingleFace( } } -void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) -{ - m_rayData = { ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z, - ray.getDirection().x, ray.getDirection().y, ray.getDirection().z }; - - std::vector> intersectedFaceList; - - // try - // { - // m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); - - // m_clQueue.enqueueNDRangeKernel(m_clKernelSingleRay, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - // cl::NullRange, nullptr); - // m_clQueue.finish(); - - // m_resultDistances.resize(m_meshGeometry->faces.size()); - // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - // m_resultDistances.data()); - // } - // catch (cl::Error err) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); - // } - - int closestFaceId = -1; - float minDist = std::numeric_limits::max(); - - for (int i = 0; i < m_meshGeometry->faces.size(); i++) - { - if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) - { - closestFaceId = i; - minDist = m_resultDistances[i]; - } - } - - if (m_displayInitialized && m_visual && closestFaceId != -1) - { - std::vector tmpFaceList; - - if (m_faceSelectedArray.size() <= closestFaceId) - { - m_faceSelectedArray.resize(closestFaceId + 1); - } - m_faceSelectedArray[closestFaceId] = selectMode; - - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } - } - - m_visual->setFacesInCluster(tmpFaceList); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "selectSingleFaceParallel() found face with id %d", closestFaceId); - } -} void ClusterLabelTool::selectSphereFaces( rviz_common::ViewportMouseEvent& event, bool selectMode) { - // Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - // (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); - // Ogre::Ray ray = event.panel->getViewController()->getCamera()->getCameraToViewportRay( - // (float)event.x / event.panel->getRenderWindow()->width(), - // (float)event.y / event.panel->getRenderWindow()->height() - // ); throw std::runtime_error("TODO"); - Ogre::Ray ray; - selectSphereFacesParallel(ray, selectMode); } -void ClusterLabelTool::selectSphereFacesParallel( - Ogre::Ray& ray, bool selectMode) -{ - auto raycastResult = getClosestIntersectedFaceParallel(ray); - - if (raycastResult) - { - Ogre::Vector3 sphereCenter = ray.getPoint(raycastResult->second); - - m_sphereData = { sphereCenter.x, sphereCenter.y, sphereCenter.z, raycastResult->second }; - - // try - // { - // m_clQueue.enqueueWriteBuffer(m_clSphereBuffer, CL_TRUE, 0, sizeof(float) * 4, m_sphereData.data()); - - // m_clQueue.enqueueNDRangeKernel(m_clKernelSphere, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - // cl::NullRange, nullptr); - // m_clQueue.finish(); - - // m_resultDistances.resize(m_meshGeometry->faces.size()); - // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - // m_resultDistances.data()); - // } - // catch (cl::Error err) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); - // } - - for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) - { - // if face is inside sphere, select it - if (m_resultDistances[faceId] > 0) - { - if (m_faceSelectedArray.size() <= faceId) - { - m_faceSelectedArray.resize(faceId + 1); - } - m_faceSelectedArray[faceId] = selectMode; - } - } - - if (m_displayInitialized && m_visual) - { - std::vector tmpFaceList; - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } - } - - m_visual->setFacesInCluster(tmpFaceList); - } - } -} - -boost::optional> ClusterLabelTool::getClosestIntersectedFaceParallel( - Ogre::Ray& ray) -{ - m_rayData = { ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z, - ray.getDirection().x, ray.getDirection().y, ray.getDirection().z }; - - // try - // { - // m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); - - // m_clQueue.enqueueNDRangeKernel(m_clKernelSingleRay, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - // cl::NullRange, nullptr); - // m_clQueue.finish(); - - // m_resultDistances.resize(m_meshGeometry->faces.size()); - // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - // m_resultDistances.data()); - // } - // catch (cl::Error err) - // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); - // } - - uint32_t closestFaceId; - bool faceFound = false; - float minDist = std::numeric_limits::max(); - - for (uint32_t i = 0; i < m_meshGeometry->faces.size(); i++) - { - if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) - { - closestFaceId = i; - faceFound = true; - minDist = m_resultDistances[i]; - } - } - - if (faceFound) - { - return std::make_pair(closestFaceId, minDist); - } - else - { - return {}; - } -} void ClusterLabelTool::publishLabel(std::string label) { @@ -765,40 +447,48 @@ void ClusterLabelTool::publishLabel(std::string label) // Handling mouse event and mark the clicked faces int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { - if (event.leftDown() && event.control()) + /* + * Left Mouse is to select something, Right Mouse is to deselect something. + * + * Modifiers: + * + * None: Mark all faces inside a circle around the current mouse position + * Shift: Create a rectangle and mark all faces inside on mouse up + */ + + + if (event.leftDown() && event.shift()) + { + m_multipleSelect = true; + selectionBoxStart(event); + } + else if (event.leftDown()) // Option without modifier needs to come last { m_singleSelect = true; - selectSphereFaces(event, true); + selectSingleFace(event, true); } else if (event.leftUp() && m_singleSelect) { m_singleSelect = false; - selectSphereFaces(event, true); } - else if (event.rightDown() && event.control()) - { - m_singleDeselect = true; - selectSphereFaces(event, false); - } - else if (event.rightUp() && m_singleDeselect) + else if (event.leftUp() && m_multipleSelect) { - m_singleDeselect = false; - selectSphereFaces(event, false); + m_multipleSelect = false; + selectMultipleFaces(event, true); } - else if (event.leftDown()) + else if (event.rightDown() && event.shift()) { m_multipleSelect = true; selectionBoxStart(event); } - else if (event.leftUp() && m_multipleSelect) + else if (event.rightDown()) { - m_multipleSelect = false; - selectMultipleFaces(event, true); + m_singleSelect = true; + selectSingleFace(event, false); } - else if (event.rightDown()) + else if (event.rightUp() && m_singleSelect) { - m_multipleSelect = true; - selectionBoxStart(event); + m_singleSelect = false; } else if (event.rightUp() && m_multipleSelect) { @@ -809,14 +499,6 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { selectionBoxMove(event); } - else if (m_singleSelect) - { - selectSphereFaces(event, true); - } - else if (m_singleDeselect) - { - selectSphereFaces(event, false); - } return Render; } @@ -849,6 +531,107 @@ void ClusterLabelTool::resetVisual() m_visual.reset(); } +Ogre::Image ClusterLabelTool::renderMeshWithFaceID() +{ + // Attach the mesh to scene node + m_selectionSceneNode->attachObject(m_selectionMesh); + + // Get the current RViz camera + Ogre::Camera* camera = context_->getViewManager()->getCurrent()->getCamera(); + + // Setup the render target to be the texture + Ogre::RenderTexture* render_texture = m_selectionTexture->getBuffer()->getRenderTarget(); + Ogre::Viewport* viewport = render_texture->addViewport(camera); + viewport->setClearEveryFrame(true); + viewport->setOverlaysEnabled(false); + viewport->setBackgroundColour(Ogre::ColourValue::White); + viewport->setVisibilityMask(m_selectionVisibilityBit); + + // Actually render offscreen + render_texture->update(); + + // Copy the data to ram + Ogre::Image img(Ogre::PF_R8G8B8, render_texture->getWidth(), render_texture->getHeight()); + Ogre::PixelBox pixels = img.getPixelBox(); + render_texture->copyContentsToMemory(pixels, pixels); + + // Detach the rviz camera + render_texture->removeAllViewports(); + // Prevent our copied Mesh to be visible in the RViz Camera + m_selectionSceneNode->detachAllObjects(); + + return img; +} + + +void ClusterLabelTool::updateSelectionMesh() +{ + // For GPU accelerated picking we need per face attributes in the shader. + // Sadly Ogre 1.12 does not have a way to do this. + // Solution: Create a renderable geometry with 3 vertices per face and + // add a vertex property with the original face_id + // Then we can write a shader to write the face id to rendered pixels. + if (nullptr == m_selectionMesh) + { + m_selectionMesh = scene_manager_->createManualObject("ClusterLabelToolPickingMesh"); + m_selectionMesh->setDynamic(false); + m_selectionMesh->begin(m_selectionBoxMaterial, Ogre::RenderOperation::OT_TRIANGLE_LIST); + } + else + { + m_selectionMesh->beginUpdate(0); + } + + // We can only use RGB (24) bits to to represent triangle id + const uint32_t MAX_NUM_FACES = std::pow(2, 24); + if (MAX_NUM_FACES <= m_meshGeometry->faces.size()) + { + RCLCPP_WARN( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "ClusterLabelTool: Mesh to label has more than the supported number of triangles (%u). This can lead to faces not being selected!", + MAX_NUM_FACES + ); + } + + for (uint32_t faceID = 0; faceID < m_meshGeometry->faces.size(); faceID++) + { + const auto& face = m_meshGeometry->faces[faceID]; + for (int i = 0; i < 3; i++) + { + const auto& v = m_meshGeometry->vertices[face.vertexIndices[i]]; + m_selectionMesh->position(v.x, v.y, v.z); + Ogre::ColourValue color; + color.setAsARGB(faceID); + color.a = 1.0; + m_selectionMesh->colour(color); + } + m_selectionMesh->triangle(faceID * 3 + 0, faceID * 3 + 1, faceID * 3 + 2); + } + m_selectionMesh->end(); + m_selectionMesh->setVisibilityFlags(m_selectionVisibilityBit); + m_selectionMesh->setMaterial(0, m_selectionMaterial); + + // Remove the selection node from the previous parent + if (auto* parent = m_selectionSceneNode->getParentSceneNode()) + { + parent->removeChild(m_selectionSceneNode); + } + + // TODO: This could or should be MeshDisplays scene node instead of the ClusterLabelDisplays + // Or the ClusterLabelDisplay also needs to honor the map to fixed frame transform + + // Add our scene node below the displays scene node + const auto& c = m_display->getSceneNode()->getChildren(); + if (c.end() == std::find(c.begin(), c.end(), m_selectionSceneNode)) + { + m_display->getSceneNode()->addChild(m_selectionSceneNode); + } + RCLCPP_INFO( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "ClusterLabelTool: Load Mesh with %lu faces", m_meshGeometry->faces.size() + ); +} + } // End namespace rviz_mesh_tools_plugins From 04b53ab91953d4a97097438e6919c399f50b0cfc Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Fri, 23 May 2025 18:53:45 +0200 Subject: [PATCH 05/12] Add circle brush tool when using ctrl with left or right mouse --- .../ClusterLabelTool.hpp | 36 ++- .../src/ClusterLabelDisplay.cpp | 13 +- .../src/ClusterLabelTool.cpp | 220 ++++++++++++++---- 3 files changed, 197 insertions(+), 72 deletions(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 4366826..5c1b7b2 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -181,10 +181,10 @@ class ClusterLabelTool : public rviz_common::Tool void setVisual(std::shared_ptr visual); /** - * @brief Adjust the sphere size for the brush tool - * @param size The sphere size + * @brief Adjust the circle size for the brush tool + * @param size The circle diameter in screen Pixels */ - void setSphereSize(float size); + void setBrushSize(float size); public Q_SLOTS: @@ -213,20 +213,19 @@ public Q_SLOTS: private: std::vector m_selectedFaces; std::vector m_faceSelectedArray; - bool m_displayInitialized; ClusterLabelDisplay* m_display; std::shared_ptr m_visual; std::shared_ptr m_meshGeometry; - float m_sphereSize = 1.0f; + float m_brushSize = 1.0f; // Selection Box - rviz_common::DisplayContext* m_displayContext; Ogre::SceneNode* m_sceneNode; Ogre::ManualObject* m_selectionBox; Ogre::MaterialPtr m_selectionBoxMaterial; - - // rviz_common::ViewportMouseEvent m_evt_start; - // rviz_common::ViewportMouseEvent m_evt_stop; + + // Selection Circle + Ogre::ManualObject* m_selectionCircle; + Ogre::SceneNode* m_selectionCircleNode; int m_bb_x1; int m_bb_y1; @@ -234,29 +233,27 @@ public Q_SLOTS: int m_bb_y2; rviz_common::RenderPanel* m_evt_panel; - + + // Selection Modes bool m_multipleSelect = false; bool m_singleSelect = false; - bool m_singleDeselect = false; + bool m_circleSelect = false; + // Select = true Deselect = false + bool m_selectionMode = false; std::vector m_vertexPositions; + void initSelectionCircle(); + void updateSelectionCircle(rviz_common::ViewportMouseEvent& event); void updateSelectionBox(); void selectionBoxStart(rviz_common::ViewportMouseEvent& event); void selectionBoxMove(rviz_common::ViewportMouseEvent& event); void selectMultipleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); void selectSingleFace(rviz_common::ViewportMouseEvent& event, bool selectMode); - void selectSphereFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); + void selectCircleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); rclcpp::Publisher::SharedPtr m_labelPublisher; - std::vector m_vertexData; - std::array m_rayData; - std::array m_sphereData; - std::array m_startNormalData; - std::vector m_boxData; - std::vector m_resultDistances; - /** * @brief Renders the current Mesh to an Offscreen Buffer using the FaceIDs as colors. * @@ -276,6 +273,7 @@ public Q_SLOTS: Ogre::MaterialPtr m_selectionMaterial; Ogre::ManualObject* m_selectionMesh; Ogre::SceneNode* m_selectionSceneNode; + // Used to render only the selectionMesh to the offscreen Texture uint32_t m_selectionVisibilityBit; }; } // end namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 40cc09c..2281167 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -111,7 +111,7 @@ ClusterLabelDisplay::ClusterLabelDisplay() m_colorsProperty = new rviz_common::properties::Property("Colors", "", "colors", this, SLOT(updateColors()), this); m_colorsProperty->setReadOnly(true); m_sphereSizeProperty = - new rviz_common::properties::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); + new rviz_common::properties::FloatProperty("Brush Size", 50.0f, "Brush Size", this, SLOT(updateSphereSize()), this); m_phantomVisualProperty = new rviz_common::properties::BoolProperty("Show Phantom", false, "Show a transparent silhouette of the whole mesh to help with " "labeling", @@ -167,7 +167,10 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector void ClusterLabelDisplay::onInitialize() { // Look for an existing label tool or create a new one - getOrCreateLabelTool(); + ClusterLabelTool* tool = getOrCreateLabelTool(); + + // Set the Tool related configuration + tool->setBrushSize(m_sphereSizeProperty->getFloat()); } void ClusterLabelDisplay::onEnable() @@ -263,7 +266,7 @@ void ClusterLabelDisplay::updateSphereSize() { if (auto* tool = getOrCreateLabelTool()) { - tool->setSphereSize(m_sphereSizeProperty->getFloat()); + tool->setBrushSize(m_sphereSizeProperty->getFloat()); } } @@ -374,12 +377,8 @@ ClusterLabelTool* ClusterLabelDisplay::getOrCreateLabelTool() RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not create ClusterLabelTool"); return nullptr; } - - // Set all settings the ClusterLabelTool needs to know - tool->setSphereSize(m_sphereSizeProperty->getFloat()); } - return tool; } diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 9dbf292..1ddae18 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -55,10 +55,6 @@ #include #include -#include -#include - -#include #include #include @@ -86,15 +82,11 @@ using std::stringstream; namespace rviz_mesh_tools_plugins { -// #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" - - ClusterLabelTool::ClusterLabelTool() :rviz_common::Tool() -,m_displayInitialized(false) -// ,m_evt_start(nullptr, (QMouseEvent*)nullptr, 0, 0) -// ,m_evt_stop(nullptr, (QMouseEvent*)nullptr, 0, 0) +, m_display(nullptr) +, m_selectionCircle(nullptr) , m_selectionMesh(nullptr) { shortcut_key_ = 'l'; @@ -103,12 +95,21 @@ ClusterLabelTool::ClusterLabelTool() ClusterLabelTool::~ClusterLabelTool() { m_selectedFaces.clear(); - context_->getSceneManager()->destroyManualObject(m_selectionBox->getName()); - context_->getSceneManager()->destroyManualObject(m_selectionBoxMaterial->getName()); - context_->getSceneManager()->destroySceneNode(m_sceneNode); - context_->getSceneManager()->destroySceneNode(m_selectionSceneNode); - context_->getSceneManager()->destroyManualObject(m_selectionMesh); + scene_manager_->destroyManualObject(m_selectionBox->getName()); + scene_manager_->destroyManualObject(m_selectionBoxMaterial->getName()); + scene_manager_->destroyManualObject(m_selectionCircle); + scene_manager_->destroyManualObject(m_selectionMesh); + scene_manager_->destroySceneNode(m_sceneNode); + scene_manager_->destroySceneNode(m_selectionSceneNode); + scene_manager_->destroySceneNode(m_selectionCircleNode); context_->visibilityBits()->freeBits(m_selectionVisibilityBit); + + m_display = nullptr; + m_selectionCircle = nullptr; + m_selectionCircleNode = nullptr; + m_evt_panel = nullptr; + m_selectionMesh = nullptr; + m_selectionSceneNode = nullptr; } // onInitialize() is called by the superclass after scene_manager_ and @@ -128,6 +129,7 @@ void ClusterLabelTool::onInitialize() m_selectionBox->setUseIdentityProjection(true); m_selectionBox->setUseIdentityView(true); m_selectionBox->setQueryFlags(0); + m_selectionBox->setBoundingBox(Ogre::AxisAlignedBox::BOX_INFINITE); m_sceneNode->attachObject(m_selectionBox); m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().getByName( @@ -146,8 +148,12 @@ void ClusterLabelTool::onInitialize() m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); - m_selectionVisibilityBit = context_->visibilityBits()->allocBit(); + // Circle overlay for the circular selection + // The SceneNode is used to move and scale the circle + m_selectionCircleNode = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode(); + initSelectionCircle(); + m_selectionVisibilityBit = context_->visibilityBits()->allocBit(); // Setup texture for accelerated picking // We render the scene in a 1280x720 resolution // There should only ever be one instance of this Tool so we can reuse the texture @@ -207,10 +213,10 @@ void ClusterLabelTool::setVisual(std::shared_ptr visual) } } -void ClusterLabelTool::setSphereSize(float size) +void ClusterLabelTool::setBrushSize(float size) { // m_clKernelSphere.setArg(3, size); - m_sphereSize = size; + m_brushSize = size; } void ClusterLabelTool::activate() @@ -227,24 +233,6 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) m_meshGeometry = m_display->getGeometry(); m_faceSelectedArray.resize(m_meshGeometry->faces.size()); std::fill(m_faceSelectedArray.begin(), m_faceSelectedArray.end(), false); - m_displayInitialized = true; - - m_vertexData.reserve(m_meshGeometry->faces.size() * 3 * 3); - - for (uint32_t faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) - { - for (uint32_t i = 0; i < 3; i++) - { - uint32_t vertexId = m_meshGeometry->faces[faceId].vertexIndices[i]; - Ogre::Vector3 vertexPos(m_meshGeometry->vertices[vertexId].x, m_meshGeometry->vertices[vertexId].y, - m_meshGeometry->vertices[vertexId].z); - m_vertexPositions.push_back(vertexPos); - - m_vertexData.push_back(m_meshGeometry->vertices[vertexId].x); - m_vertexData.push_back(m_meshGeometry->vertices[vertexId].y); - m_vertexData.push_back(m_meshGeometry->vertices[vertexId].z); - } - } // Prepare for GPU accelerated selection updateSelectionMesh(); @@ -275,10 +263,10 @@ void ClusterLabelTool::updateSelectionBox() m_selectionBox->clear(); m_selectionBox->begin(m_selectionBoxMaterial->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); - m_selectionBox->position(left, top, -1); - m_selectionBox->position(right, top, -1); - m_selectionBox->position(right, bottom, -1); - m_selectionBox->position(left, bottom, -1); + m_selectionBox->position(left, top, 0.0); + m_selectionBox->position(right, top, 0.0); + m_selectionBox->position(right, bottom, 0.0); + m_selectionBox->position(left, bottom, 0.0); m_selectionBox->triangle(0, 1, 2); m_selectionBox->triangle(0, 2, 3); m_selectionBox->end(); @@ -315,7 +303,6 @@ void ClusterLabelTool::selectMultipleFaces( m_selectionBox->setVisible(false); Ogre::Image img = renderMeshWithFaceID(); - img.save("test.png"); // Translate the selection box to uv coordinates uint32_t x0 = std::min(m_bb_x1, m_bb_x2); @@ -394,7 +381,7 @@ void ClusterLabelTool::selectSingleFace( { // std::cout << "selectSingleFace- HIT!" << std::endl; - if (m_displayInitialized && m_visual) + if (m_display && m_visual) { std::vector tmpFaceList; @@ -423,10 +410,72 @@ void ClusterLabelTool::selectSingleFace( } -void ClusterLabelTool::selectSphereFaces( +void ClusterLabelTool::selectCircleFaces( rviz_common::ViewportMouseEvent& event, bool selectMode) { - throw std::runtime_error("TODO"); + // Get the selection buffer + Ogre::Image buffer = renderMeshWithFaceID(); + + // The diameter of the selection circle in RViz Display pixels + // This needs to be mapped using uv coordinates because the height and width + // if the selection buffer image are fixed. + const float& diameter = m_brushSize; + const float du = (float) diameter / event.panel->width(); + const float dv = (float) diameter / event.panel->height(); + + // Position of the curser in uv coordinates + const float u = (float) event.x / event.panel->width(); + const float v = (float) event.y / event.panel->height(); + + // Position of the curser in img coordinates + const int32_t cx = u * buffer.getWidth(); + const int32_t cy = v * buffer.getHeight(); + + // X/Y Diameter in img coordinates + const int32_t dx = du * buffer.getWidth(); + const int32_t dy = dv * buffer.getHeight(); + + // Bounding rect in img coordinates + const uint32_t x_min = std::clamp(cx - dx / 2, 0, buffer.getWidth()); + const uint32_t x_max = std::clamp(cx + dx / 2, 0, buffer.getWidth()); + const uint32_t y_min = std::clamp(cy - dy / 2, 0, buffer.getHeight()); + const uint32_t y_max = std::clamp(cy + dy / 2, 0, buffer.getHeight()); + + const int32_t rx = dx / 2; + const int32_t ry = dy / 2; + for (uint32_t y = y_min; y < y_max; y++) + { + for (uint32_t x = x_min; x < x_max; x++) + { + // Check if the point lies inside the img space ellipse + const int32_t dx = x - cx; + const int32_t dy = y - cy; + + const float t = (std::pow(dx, 2) / std::pow(rx, 2)) + (std::pow(dy, 2) / std::pow(ry, 2)); + if (t >= 1.0) + { + continue; + } + + // Point is inside the ellipse + Ogre::ColourValue color = buffer.getColourAt(x, y, 0); + color.a = 0.0; + const uint32_t face_id = color.getAsARGB(); + m_faceSelectedArray[face_id] = selectMode; + } + } + + // Update the visual + std::vector tmpFaceList; + for(size_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + tmpFaceList.push_back(faceId); + } + } + + m_visual->setFacesInCluster(tmpFaceList); } @@ -456,12 +505,29 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) * Shift: Create a rectangle and mark all faces inside on mouse up */ + // Show circle overlay when control is pressed + if (event.control()) + { + updateSelectionCircle(event); + m_selectionCircle->setVisible(true); + } + else + { + m_selectionCircle->setVisible(false); + } + if (event.leftDown() && event.shift()) { m_multipleSelect = true; selectionBoxStart(event); } + else if (event.leftDown() && event.control()) + { + m_circleSelect = true; + m_selectionMode = true; + selectCircleFaces(event, true); + } else if (event.leftDown()) // Option without modifier needs to come last { m_singleSelect = true; @@ -476,11 +542,21 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) m_multipleSelect = false; selectMultipleFaces(event, true); } + else if (event.leftUp() && m_circleSelect) + { + m_circleSelect = false; + } else if (event.rightDown() && event.shift()) { m_multipleSelect = true; selectionBoxStart(event); } + else if (event.rightDown() && event.control()) + { + m_circleSelect = true; + m_selectionMode = false; + selectCircleFaces(event, false); + } else if (event.rightDown()) { m_singleSelect = true; @@ -495,10 +571,18 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) m_multipleSelect = false; selectMultipleFaces(event, false); } + else if (event.rightUp() && m_circleSelect) + { + m_circleSelect = false; + } else if (m_multipleSelect) { selectionBoxMove(event); } + else if (m_circleSelect) + { + selectCircleFaces(event, m_selectionMode); + } return Render; } @@ -507,7 +591,7 @@ std::vector ClusterLabelTool::getSelectedFaces() { std::vector faceList; - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + for (uint32_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) { if (m_faceSelectedArray[faceId]) { @@ -632,8 +716,52 @@ void ClusterLabelTool::updateSelectionMesh() ); } -} // End namespace rviz_mesh_tools_plugins +void ClusterLabelTool::updateSelectionCircle(rviz_common::ViewportMouseEvent& event) +{ + const float x = (((float) event.x / event.panel->width()) * 2.0f) - 1.0f; + const float y = (((float) event.y / event.panel->height()) * 2.0f) - 1.0f; + + // Scaling the circle to have the wanted dimensions in pixel space + const float sx = ((float) m_brushSize / event.panel->width()); + const float sy = ((float) m_brushSize / event.panel->height()); + + m_selectionCircleNode->setPosition(x, -y, 0.0f); + m_selectionCircleNode->setScale({sx, sy, 1.0}); +} + + +void ClusterLabelTool::initSelectionCircle() +{ + // Already initialized + if (m_selectionCircle) + { + return; + } + + m_selectionCircle = context_->getSceneManager()->createManualObject("ClusterLabelTool_SelectionCircle"); + m_selectionCircle->begin(m_selectionBoxMaterial, Ogre::RenderOperation::OT_LINE_STRIP); + m_selectionCircle->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY); + m_selectionCircle->setUseIdentityProjection(true); + m_selectionCircle->setUseIdentityView(true); + m_selectionCircle->setQueryFlags(0); + m_selectionCircle->setBoundingBox(Ogre::AxisAlignedBox::BOX_INFINITE); + m_selectionCircle->setVisibilityFlags(context_->getDefaultVisibilityBit()); + m_selectionCircle->setVisible(false); + m_selectionCircleNode->attachObject(m_selectionCircle); + + // Generate 2D circle + for (uint32_t i = 0; i < 60; i++) + { + const float rad = ((float) i / (60 - 1)) * 2.0 * M_PIf; + const float x = std::cos(rad); + const float y = std::sin(rad); + m_selectionCircle->position(x, y, 0.0); + } + m_selectionCircle->end(); + m_selectionCircle->setMaterial(0, m_selectionBoxMaterial); +} +} // End namespace rviz_mesh_tools_plugins #include From c33c92fbe014d4243109fd26afcda23dec3c7705 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Fri, 23 May 2025 19:25:53 +0200 Subject: [PATCH 06/12] Removed CLUtils because OpenCL is no longer needed. Fixed some segfaults --- rviz_mesh_tools_plugins/CMakeLists.txt | 6 - .../rviz_mesh_tools_plugins/CLUtil.hpp | 476 ------------------ .../ClusterLabelTool.hpp | 8 - .../src/ClusterLabelTool.cpp | 91 +++- 4 files changed, 72 insertions(+), 509 deletions(-) delete mode 100644 rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 3114fb9..ed34323 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -37,9 +37,6 @@ find_package(Boost REQUIRED COMPONENTS system filesystem) find_package(HDF5 REQUIRED COMPONENTS C CXX HL) # find_package(assimp REQUIRED) -# This needs to be optional for non CL devices -# find_package(OpenCL 2 REQUIRED) - # include_directories(${ASSIMP_INCLUDE_DIRS}) # LVR2 includes HighFive that we need here @@ -57,7 +54,6 @@ include_directories( ${LVR2_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${HDF5_INCLUDE_DIRS} - ${OpenCL_INCLUDE_DIRS} ) set(SOURCE_FILES @@ -84,7 +80,6 @@ set(MOC_HEADER_FILES include/rviz_mesh_tools_plugins/MeshDisplay.hpp include/rviz_mesh_tools_plugins/MeshVisual.hpp include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp - include/rviz_mesh_tools_plugins/CLUtil.hpp include/rviz_mesh_tools_plugins/RvizFileProperty.hpp include/rviz_mesh_tools_plugins/MeshPoseTool.hpp include/rviz_mesh_tools_plugins/MeshGoalTool.hpp @@ -116,7 +111,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PRIVATE ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} - ${OpenCL_LIBRARIES} ) target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_MESH_TOOLS_PLUGINS_BUILDING_LIBRARY")# diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp deleted file mode 100644 index 100d380..0000000 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp +++ /dev/null @@ -1,476 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Robot Operating System code by the University of Osnabrück - * Copyright (c) 2015, University of Osnabrück - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above - * copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED - * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * - * - * CLUtil.hpp - * - * - * authors: - * - * Kristin Schmidt - */ - -#ifndef CL_UTIL_HPP -#define CL_UTIL_HPP - -#include - -namespace rviz_mesh_tools_plugins -{ -/** - * @class CLUtil - * @brief Utility class for getting human-readable messages from OpenCL error codes - */ -class CLUtil -{ -public: - /** - * @brief Returns the error string to a given OpenCL error code - * @param error The error code - * @return The name of the error - */ - static inline const char* getErrorString(cl_int error) - { - switch (error) - { - // run-time and JIT compiler errors - case 0: - return "CL_SUCCESS"; - case -1: - return "CL_DEVICE_NOT_FOUND"; - case -2: - return "CL_DEVICE_NOT_AVAILABLE"; - case -3: - return "CL_COMPILER_NOT_AVAILABLE"; - case -4: - return "CL_MEM_OBJECT_ALLOCATION_FAILURE"; - case -5: - return "CL_OUT_OF_RESOURCES"; - case -6: - return "CL_OUT_OF_HOST_MEMORY"; - case -7: - return "CL_PROFILING_INFO_NOT_AVAILABLE"; - case -8: - return "CL_MEM_COPY_OVERLAP"; - case -9: - return "CL_IMAGE_FORMAT_MISMATCH"; - case -10: - return "CL_IMAGE_FORMAT_NOT_SUPPORTED"; - case -11: - return "CL_BUILD_PROGRAM_FAILURE"; - case -12: - return "CL_MAP_FAILURE"; - case -13: - return "CL_MISALIGNED_SUB_BUFFER_OFFSET"; - case -14: - return "CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST"; - case -15: - return "CL_COMPILE_PROGRAM_FAILURE"; - case -16: - return "CL_LINKER_NOT_AVAILABLE"; - case -17: - return "CL_LINK_PROGRAM_FAILURE"; - case -18: - return "CL_DEVICE_PARTITION_FAILED"; - case -19: - return "CL_KERNEL_ARG_INFO_NOT_AVAILABLE"; - - // compile-time errors - case -30: - return "CL_INVALID_VALUE"; - case -31: - return "CL_INVALID_DEVICE_TYPE"; - case -32: - return "CL_INVALID_PLATFORM"; - case -33: - return "CL_INVALID_DEVICE"; - case -34: - return "CL_INVALID_CONTEXT"; - case -35: - return "CL_INVALID_QUEUE_PROPERTIES"; - case -36: - return "CL_INVALID_COMMAND_QUEUE"; - case -37: - return "CL_INVALID_HOST_PTR"; - case -38: - return "CL_INVALID_MEM_OBJECT"; - case -39: - return "CL_INVALID_IMAGE_FORMAT_DESCRIPTOR"; - case -40: - return "CL_INVALID_IMAGE_SIZE"; - case -41: - return "CL_INVALID_SAMPLER"; - case -42: - return "CL_INVALID_BINARY"; - case -43: - return "CL_INVALID_BUILD_OPTIONS"; - case -44: - return "CL_INVALID_PROGRAM"; - case -45: - return "CL_INVALID_PROGRAM_EXECUTABLE"; - case -46: - return "CL_INVALID_KERNEL_NAME"; - case -47: - return "CL_INVALID_KERNEL_DEFINITION"; - case -48: - return "CL_INVALID_KERNEL"; - case -49: - return "CL_INVALID_ARG_INDEX"; - case -50: - return "CL_INVALID_ARG_VALUE"; - case -51: - return "CL_INVALID_ARG_SIZE"; - case -52: - return "CL_INVALID_KERNEL_ARGS"; - case -53: - return "CL_INVALID_WORK_DIMENSION"; - case -54: - return "CL_INVALID_WORK_GROUP_SIZE"; - case -55: - return "CL_INVALID_WORK_ITEM_SIZE"; - case -56: - return "CL_INVALID_GLOBAL_OFFSET"; - case -57: - return "CL_INVALID_EVENT_WAIT_LIST"; - case -58: - return "CL_INVALID_EVENT"; - case -59: - return "CL_INVALID_OPERATION"; - case -60: - return "CL_INVALID_GL_OBJECT"; - case -61: - return "CL_INVALID_BUFFER_SIZE"; - case -62: - return "CL_INVALID_MIP_LEVEL"; - case -63: - return "CL_INVALID_GLOBAL_WORK_SIZE"; - case -64: - return "CL_INVALID_PROPERTY"; - case -65: - return "CL_INVALID_IMAGE_DESCRIPTOR"; - case -66: - return "CL_INVALID_COMPILER_OPTIONS"; - case -67: - return "CL_INVALID_LINKER_OPTIONS"; - case -68: - return "CL_INVALID_DEVICE_PARTITION_COUNT"; - - // extension errors - case -1000: - return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR"; - case -1001: - return "CL_PLATFORM_NOT_FOUND_KHR"; - case -1002: - return "CL_INVALID_D3D10_DEVICE_KHR"; - case -1003: - return "CL_INVALID_D3D10_RESOURCE_KHR"; - case -1004: - return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR"; - case -1005: - return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR"; - default: - return "Unknown OpenCL error"; - } - }; - - /** - * @brief Returns a description to a given OpenCL error code - * @param error The error code - * @return A description of the error - */ - static inline const char* getErrorDescription(cl_int error) - { - switch (error) - { - // run-time and JIT compiler errors - case 0: - return "Indicates that the function executed successfully."; - case -1: - return "Returned by clGetDeviceIDs and clCreateContextFromType if no OpenCL devices that match " - "the specified devices were found."; - case -2: - return "Returned by clCreateContext and clCreateContextFromType if the specified device is not " - "currently available."; - case -3: - return "Returned by clBuildProgram if the parameter program is created with " - "clCreateProgramWithSource and a compiler is not available. For example " - "CL_DEVICE_COMPILER_AVAILABLE is set to CL_FALSE."; - case -4: - return "Returned by the functions listed below if there is a failure to allocate memory for data " - "store associated with image or buffer objects specified as arguments to kernel. The exact " - "condition that generates this error depends on the calling function. Refer to the " - "function for more information. \n" - "Returned by the following functions: clCreateBuffer, clEnqueueReadBuffer, " - "clEnqueueWriteBuffer, clEnqueueCopyBuffer, clCreateImage2D, clCreateImage3D, " - "clEnqueueReadImage, clEnqueueWriteImage, clEnqueueCopyImage, clEnqueueCopyImageToBuffer, " - "clEnqueueCopyBufferToImage, clEnqueueMapBuffer, clEnqueueMapImage, " - "clEnqueueNDRangeKernel, clEnqueueTask, and clEnqueueNativeKernel "; - case -5: - return "Returned by clEnqueueNDRangeKernel, clEnqueueTask, and clEnqueueNativeKernel in the event " - "of a failure to queue the execution instance of kernel on the command-queue because of " - "insufficient resources needed to execute the kernel."; - case -6: - return "Returned by the functions listed below in the event of a failure to allocate resources " - "required by the OpenCL implementation on the host. The exact condition that generates " - "this error depends on the calling function. Refer to the function for more information. "; - case -7: - return "Returned by clGetEventProfilingInfo if the CL_QUEUE_PROFILING_ENABLE flag is not set for " - "the command-queue and the profiling information is currently not available (because the " - "command identified by event has not completed)."; - case -8: - return "Returned by clEnqueueCopyBuffer and clEnqueueCopyImage if the source and destination " - "images are the same image (or the source and destination buffers are the same buffer), " - "and the source and destination regions overlap."; - case -9: - return "Returned by clEnqueueCopyImage if the specified source and destination images are not " - "valid image objects."; - case -10: - return "Returned by clCreateImage2D and clCreateImage3D if the specified image format is not " - "supported."; - case -11: - return "Returned by clBuildProgram if there is a failure to build the program executable. This " - "error will be returned if clBuildProgram does not return until the build has completed."; - case -12: - return "Returned by clEnqueueMapBuffer and clEnqueueMapImage if there is a failure to map the " - "requested region into the host address space. This error cannot occur for buffer objects " - "created with CL_MEM_USE_HOST_PTR or CL_MEM_ALLOC_HOST_PTR."; - case -13: - return "CL_MISALIGNED_SUB_BUFFER_OFFSET"; - case -14: - return "CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST"; - case -15: - return "CL_COMPILE_PROGRAM_FAILURE"; - case -16: - return "CL_LINKER_NOT_AVAILABLE"; - case -17: - return "CL_LINK_PROGRAM_FAILURE"; - case -18: - return "CL_DEVICE_PARTITION_FAILED"; - case -19: - return "CL_KERNEL_ARG_INFO_NOT_AVAILABLE"; - - // compile-time errors - case -30: - return "Returned by the functions listed below if a parameter is not an expected value. The " - "exact condition that generates this error depends on the calling function. Refer to the " - "function for more information."; - case -31: - return "Returned by clGetDeviceIDs and clCreateContextFromType if device type specified is not " - "valid."; - case -32: - return "Returned by clGetPlatformInfo and clGetDeviceIDs if the specified platform is not a " - "valid platform.\n" - "Returned by clCreateContext and clCreateContextFromType if properties is NULL and no " - "platform could be selected, or if platform value specified in properties is not a valid " - "platform."; - case -33: - return "Returned by the functions listed below if the device or devices specified are not valid. " - "The exact condition that generates this error depends on the calling function. Refer to " - "the function for more information."; - case -34: - return "Returned by the functions listed below if the specified context is not a valid OpenCL " - "context, or the context associated with certain parameters are not the same. The exact " - "condition that generates this error depends on the calling function. Refer to the " - "function for more information."; - case -35: - return "Returned by clCreateCommandQueue and clSetCommandQueueProperty if specified properties " - "are valid but are not supported by the device."; - case -36: - return "Returned by the functions listed below if the specified command-queue is not a valid " - "command-queue. The exact condition that generates this error depends on the calling " - "function. Refer to the function for more information."; - case -37: - return "Returned by the functions listed below if host_ptr is NULL and CL_MEM_USE_HOST_PTR or " - "CL_MEM_COPY_HOST_PTR are set in flags or if host_ptr is not NULL but " - "CL_MEM_COPY_HOST_PTR or CL_MEM_USE_HOST_PTR are not set in flags. The exact condition " - "that generates this error depends on the calling function. Refer to the function for " - "more information.\n" - "Returned by the functions clCreateBuffer, clCreateImage2D, and clCreateImage3D. "; - case -38: - return "Returned by the functions listed below if a parameter is not a valid memory, image, or " - "buffer object. The exact condition that generates this error depends on the calling " - "function. Refer to the function for more information."; - case -39: - return "Returned by clCreateImage2D and clCreateImage3D if the image format specified is not " - "valid or is NULL.\n" - "Returned byclCreateFromGLTexture2D and clCreateFromGLTexture3D\n" - "Returned by clCreateFromGLRenderbuffer if the OpenGL renderbuffer internal format does " - "not map to a supported OpenCL image format. "; - case -40: - return "Returned by clCreateImage2D if the specified image width or height are 0 or if they " - "exceed values specified in CL_DEVICE_IMAGE2D_MAX_WIDTH or CL_DEVICE_IMAGE2D_MAX_HEIGHT " - "respectively for all devices in context, or if the specified image row pitch does not " - "follow rules described for clCreateImage2D.\n" - "Returned by clCreateImage3D if the specified image width or height are 0 or if the image " - "depth is <= 1, or if they exceed values specified in CL_DEVICE_IMAGE3D_MAX_WIDTH, " - "CL_DEVICE_IMAGE3D_MAX_HEIGHT or CL_DEVICE_IMAGE3D_MAX_DEPTH respectively for all devices " - "in context, or if the image row pitch and image slice pitch do not follow rules " - "described for clCreateImage3D. "; - case -41: - return "Returned by clRetainSampler, clReleaseSampler, and clGetSamplerInfo if the specified " - "sampler is not a valid sampler object.\n" - "Returned by clSetKernelArg for an argument declared to be of type sampler_t when the " - "specified arg_value is not a valid sampler object. "; - case -42: - return "Returned by clBuildProgram and clCreateProgramWithBinary if the program binary is not a " - "valid binary for the specified device."; - case -43: - return "Returned by clBuildProgram if the specified build options are invalid."; - case -44: - return "Returned by clCreateKernel if there is no successfully built executable for program, and " - "returned by clCreateKernelsInProgram if there is no device in program.\n" - "Returned by clEnqueueNDRangeKernel and clEnqueueTask if there is no successfully built " - "program executable available for device associated with command_queue. "; - case -45: - return "Returned by clCreateKernel if there is no successfully built executable for program, and " - "returned by clCreateKernelsInProgram if there is no device in program.\n" - "Returned by clEnqueueNDRangeKernel and clEnqueueTask if there is no successfully built " - "program executable available for device associated with command_queue. "; - case -46: - return "Returned by clCreateKernel if the specified kernel name is not found in program."; - case -47: - return "Returned by clCreateKernel if the function definition for __kernel function given by " - "kernel_name such as the number of arguments, the argument types are not the same for all " - "devices for which the program executable has been built."; - case -48: - return "Returned by the functions listed below if the specified kernel is not a valid kernel " - "object. The exact condition that generates this error depends on the calling function. " - "Refer to the function for more information."; - case -49: - return "Returned by clSetKernelArg if an invalid argument index is specified."; - case -50: - return "Returned by clSetKernelArg if the argument value specified is NULL for an argument that " - "is not declared with the __local qualifier or vice-versa."; - case -51: - return "Returned by clSetKernelArg if argument size specified (arg_size) does not match the size " - "of the data type for an argument that is not a memory object, or if the argument is a " - "memory object and arg_size != sizeof(cl_mem) or if arg_size is zero and the argument is " - "declared with the __local qualifier or if the argument is a sampler and arg_size != " - "sizeof(cl_sampler)."; - case -52: - return "Returned by clEnqueueNDRangeKernel and clEnqueueTask if the kernel argument values have " - "not been specified."; - case -53: - return "Returned by clEnqueueNDRangeKernel if work_dim is not a valid value."; - case -54: - return "Returned by clEnqueueNDRangeKernel and clEnqueueTask if local_work_size is specified and " - "number of workitems specified by global_work_size is not evenly divisible by size of " - "work-group given by local_work_size or does not match the work-group size specified for " - "kernel using the __attribute__((reqd_work_group_size(X, Y, Z))) qualifier in program " - "source."; - case -55: - return "Returned by clEnqueueNDRangeKernel if the number of work-items specified in any of " - "local_work_size... [0]... local_work_size[work_dim - 1] is greater than the " - "corresponding values specified by CL_DEVICE_MAX_WORK_ITEM_SIZES[0],... " - "CL_DEVICE_MAX_WORK_ITEM_SIZES[work_dim -1]."; - case -56: - return "Returned by clEnqueueNDRangeKernel if global_work_offset is not NULL."; - case -57: - return "Returned by the functions listed below if event_wait_list is NULL and " - "num_events_in_wait_list > 0, or event_wait_list_list is not NULL and " - "num_events_in_wait_list is 0, or specified event objects are not valid events. The exact " - "condition that generates this error depends on the calling function. Refer to the " - "function for more information."; - case -58: - return "Returned by the functions listed below if the event objects specified are not valid. The " - "exact condition that generates this error depends on the calling function. Refer to the " - "function for more information."; - case -59: - return "Returned by clCreateImage2D, clCreateImage3D, and clCreateSampler if there are no " - "devices in context that support images.\n" - "Returned by clBuildProgram if the build of a program executable for any of the devices " - "specified by a previous call to clBuildProgram for program has not completed, or if " - "there are kernel objects attached to program.\n" - "Returned by clEnqueueNativeKernel if the specified device cannot execute the native " - "kernel.\n" - "Returned by clCreateFromGLTexture2D if the miplevel is less than 0.\n" - "Returned by clCreateFromGLTexture3D if 3D images are not supported by the OpenCL " - "embedded profile. "; - case -60: - return "Returned by clCreateFromGLBuffer if bufobj is not a GL buffer object or is a GL buffer " - "object but does not have an existing data store.\n" - "Returned by clCreateFromGLRenderbuffer if renderbuffer is not a GL renderbuffer object " - "or if the width or height of renderbuffer is zero.\n" - "Returned by clCreateFromGLTexture2D and clCreateFromGLTexture3D if texture is not a GL " - "texture object whose type matches texture_target, if the specified miplevel of texture " - "is not defined, or if the width, height or depth of the specified miplevel is zero.\n" - "Returned by clGetGLObjectInfo and clGetGLTextureInfo if there is no GL object or texture " - "associated with memobj.\n" - "Returned by clEnqueueAcquireGLObjects and clEnqueueReleaseGLObjects if memory objects " - "in mem_objects have not been created from a GL object(s). The exact condition that " - "generates this error depends on the calling function. "; - case -61: - return "Returned by clCreateBuffer if the value of the parameter size is 0 or is greater than " - "CL_DEVICE_MAX_MEM_ALLOC_SIZE for all devices specified in the parameter context."; - case -62: - return "CL_INVALID_MIP_LEVEL"; - case -63: - return "CL_INVALID_GLOBAL_WORK_SIZE"; - case -64: - return "CL_INVALID_PROPERTY"; - case -65: - return "CL_INVALID_IMAGE_DESCRIPTOR"; - case -66: - return "CL_INVALID_COMPILER_OPTIONS"; - case -67: - return "CL_INVALID_LINKER_OPTIONS"; - case -68: - return "CL_INVALID_DEVICE_PARTITION_COUNT"; - - // extension errors - case -1000: - return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR"; - case -1001: - return "CL_PLATFORM_NOT_FOUND_KHR"; - case -1002: - return "CL_INVALID_D3D10_DEVICE_KHR"; - case -1003: - return "CL_INVALID_D3D10_RESOURCE_KHR"; - case -1004: - return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR"; - case -1005: - return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR"; - default: - return "Unknown OpenCL error"; - } - }; -}; - -} // end namespace rviz_mesh_tools_plugins - -#endif \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 5c1b7b2..cfea617 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -52,15 +52,7 @@ #include -// TODO: Make CL optional -// enable exceptions for OpenCL -// #define CL_HPP_TARGET_OPENCL_VERSION 120 -// #define CL_HPP_MINIMUM_OPENCL_VERSION 110 -// #define CL_HPP_ENABLE_EXCEPTIONS -// #include - #include -#include #include #include #include diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 1ddae18..277265a 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -49,8 +49,6 @@ #include #include #include -#include - #include #include @@ -88,6 +86,7 @@ ClusterLabelTool::ClusterLabelTool() , m_display(nullptr) , m_selectionCircle(nullptr) , m_selectionMesh(nullptr) +, m_selectionVisibilityBit(0) { shortcut_key_ = 'l'; } @@ -95,21 +94,42 @@ ClusterLabelTool::ClusterLabelTool() ClusterLabelTool::~ClusterLabelTool() { m_selectedFaces.clear(); - scene_manager_->destroyManualObject(m_selectionBox->getName()); - scene_manager_->destroyManualObject(m_selectionBoxMaterial->getName()); - scene_manager_->destroyManualObject(m_selectionCircle); - scene_manager_->destroyManualObject(m_selectionMesh); - scene_manager_->destroySceneNode(m_sceneNode); - scene_manager_->destroySceneNode(m_selectionSceneNode); - scene_manager_->destroySceneNode(m_selectionCircleNode); + if (m_selectionBox) + { + scene_manager_->destroyManualObject(m_selectionBox->getName()); + m_selectionBox = nullptr; + } + if (m_selectionBoxMaterial) + { + scene_manager_->destroyManualObject(m_selectionBoxMaterial->getName()); + m_selectionBoxMaterial.reset(); + } + if (m_selectionCircle) + { + scene_manager_->destroyManualObject(m_selectionCircle); + m_selectionCircle = nullptr; + } + if (m_selectionMesh) + { + scene_manager_->destroyManualObject(m_selectionMesh); + m_selectionMesh = nullptr; + } + if (m_sceneNode) + { + scene_manager_->destroySceneNode(m_sceneNode); + m_sceneNode = nullptr; + } + if (m_selectionSceneNode) + { + scene_manager_->destroySceneNode(m_selectionSceneNode); + m_selectionSceneNode = nullptr; + } + if (m_selectionCircleNode) + { + scene_manager_->destroySceneNode(m_selectionCircleNode); + m_selectionCircleNode = nullptr; + } context_->visibilityBits()->freeBits(m_selectionVisibilityBit); - - m_display = nullptr; - m_selectionCircle = nullptr; - m_selectionCircleNode = nullptr; - m_evt_panel = nullptr; - m_selectionMesh = nullptr; - m_selectionSceneNode = nullptr; } // onInitialize() is called by the superclass after scene_manager_ and @@ -229,6 +249,7 @@ void ClusterLabelTool::deactivate() void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) { + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelTool::setDisplay()"); m_display = display; m_meshGeometry = m_display->getGeometry(); m_faceSelectedArray.resize(m_meshGeometry->faces.size()); @@ -300,7 +321,12 @@ void ClusterLabelTool::selectMultipleFaces( bool selectMode) { (void) event; - m_selectionBox->setVisible(false); + // No map loaded or ClusterLabel display is not active + if (!m_visual) + { + return; + } + Ogre::Image img = renderMeshWithFaceID(); @@ -365,7 +391,11 @@ void ClusterLabelTool::selectMultipleFaces( } } - m_visual->setFacesInCluster(tmpFaceList); + // Maybe the user has not loaded a map yet? + if (m_visual) + { + m_visual->setFacesInCluster(tmpFaceList); + } } @@ -413,6 +443,12 @@ void ClusterLabelTool::selectSingleFace( void ClusterLabelTool::selectCircleFaces( rviz_common::ViewportMouseEvent& event, bool selectMode) { + // No map loaded or ClusterLabel display is not active + if (!m_visual) + { + return; + } + // Get the selection buffer Ogre::Image buffer = renderMeshWithFaceID(); @@ -461,6 +497,13 @@ void ClusterLabelTool::selectCircleFaces( Ogre::ColourValue color = buffer.getColourAt(x, y, 0); color.a = 0.0; const uint32_t face_id = color.getAsARGB(); + + // Background color is white and results in an invalid face_id + if (face_id >= m_faceSelectedArray.size()) + { + continue; + } + m_faceSelectedArray[face_id] = selectMode; } } @@ -508,6 +551,9 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) // Show circle overlay when control is pressed if (event.control()) { + // Update the brush size with the wheel delta + m_brushSize = std::max(MIN_BRUSH_SIZE, m_brushSize + event.wheel_delta); + updateSelectionCircle(event); m_selectionCircle->setVisible(true); } @@ -540,6 +586,7 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) else if (event.leftUp() && m_multipleSelect) { m_multipleSelect = false; + m_selectionBox->setVisible(false); selectMultipleFaces(event, true); } else if (event.leftUp() && m_circleSelect) @@ -569,6 +616,7 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) else if (event.rightUp() && m_multipleSelect) { m_multipleSelect = false; + m_selectionBox->setVisible(false); selectMultipleFaces(event, false); } else if (event.rightUp() && m_circleSelect) @@ -657,15 +705,20 @@ void ClusterLabelTool::updateSelectionMesh() // Then we can write a shader to write the face id to rendered pixels. if (nullptr == m_selectionMesh) { + // Init the ManualObject used for offscreen rendering m_selectionMesh = scene_manager_->createManualObject("ClusterLabelToolPickingMesh"); - m_selectionMesh->setDynamic(false); + m_selectionMesh->estimateVertexCount(m_meshGeometry->faces.size() * 3); + m_selectionMesh->estimateIndexCount(m_meshGeometry->faces.size() * 3); m_selectionMesh->begin(m_selectionBoxMaterial, Ogre::RenderOperation::OT_TRIANGLE_LIST); } else { + m_selectionMesh->estimateVertexCount(m_meshGeometry->faces.size() * 3); + m_selectionMesh->estimateIndexCount(m_meshGeometry->faces.size() * 3); m_selectionMesh->beginUpdate(0); } + // We can only use RGB (24) bits to to represent triangle id const uint32_t MAX_NUM_FACES = std::pow(2, 24); if (MAX_NUM_FACES <= m_meshGeometry->faces.size()) From 97c8bc42cec5ac7a82f37cf1fa8ab3c410ab25f6 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Sat, 24 May 2025 11:13:19 +0200 Subject: [PATCH 07/12] Adjusted Mouse Wheel Brush size step --- .../rviz_mesh_tools_plugins/ClusterLabelTool.hpp | 4 ++++ rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp | 10 +++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index cfea617..1ea1db0 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -128,6 +128,10 @@ class ClusterLabelTool : public rviz_common::Tool { Q_OBJECT public: + // Constants + static constexpr float MIN_BRUSH_SIZE = 20.0f; + static constexpr float MOUSE_WHEEL_BRUSH_SIZE_STEP = 10; + /** * @brief Constructor */ diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 277265a..39781e4 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -552,7 +552,15 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) if (event.control()) { // Update the brush size with the wheel delta - m_brushSize = std::max(MIN_BRUSH_SIZE, m_brushSize + event.wheel_delta); + // In my tests one wheel step was always +-120 + if (event.wheel_delta > 0) + { + m_brushSize = std::max(MIN_BRUSH_SIZE, m_brushSize + MOUSE_WHEEL_BRUSH_SIZE_STEP); + } + else if (event.wheel_delta < 0) + { + m_brushSize = std::max(MIN_BRUSH_SIZE, m_brushSize - MOUSE_WHEEL_BRUSH_SIZE_STEP); + } updateSelectionCircle(event); m_selectionCircle->setVisible(true); From de39082bcb05917c3c4849ab7aaef6e45e93ef98 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Sat, 24 May 2025 12:11:48 +0200 Subject: [PATCH 08/12] Fix label duplication when overwriting an existing instance --- .../src/ClusterLabelDisplay.cpp | 6 +++- .../src/ClusterLabelTool.cpp | 28 +++++++++++++++---- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 16 ++++++++--- 3 files changed, 39 insertions(+), 11 deletions(-) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 2281167..a1662f4 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -226,6 +226,7 @@ void ClusterLabelDisplay::updateMap() tool->resetVisual(); } else { setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "ClusterLabel Tool could not be initialized! It will not work!"); + return; } // Now create the visuals for the loaded clusters @@ -322,7 +323,10 @@ void ClusterLabelDisplay::createVisualsFromClusterList() ss << "ClusterLabelVisual_" << i; auto visual = std::make_shared(context_, ss.str(), m_geometry); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Create visual for label '" << m_clusterList[i].name << "'"); + RCLCPP_DEBUG( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "Label Display: Create visual for label '%s' with %lu faces", m_clusterList[i].name.c_str(), m_clusterList[i].faces.size() + ); visual->setFacesInCluster(m_clusterList[i].faces); visual->setColor(getRainbowColor((++colorIndex / m_clusterList.size())), m_alphaProperty->getFloat()); m_visuals.push_back(visual); diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 39781e4..67f9bb3 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -235,7 +235,6 @@ void ClusterLabelTool::setVisual(std::shared_ptr visual) void ClusterLabelTool::setBrushSize(float size) { - // m_clKernelSphere.setArg(3, size); m_brushSize = size; } @@ -245,6 +244,8 @@ void ClusterLabelTool::activate() void ClusterLabelTool::deactivate() { + // Ensure that the Circle disappears when the tool is deactivated + m_selectionCircle->setVisible(false); } void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) @@ -252,9 +253,25 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelTool::setDisplay()"); m_display = display; m_meshGeometry = m_display->getGeometry(); + + // Allocate a buffer and init to unselected m_faceSelectedArray.resize(m_meshGeometry->faces.size()); std::fill(m_faceSelectedArray.begin(), m_faceSelectedArray.end(), false); + // If a Visual is set ensure the tools internal state is in sync with it + // If the Visual is replaced later the internal state is updated again + if (m_visual) + { + for (size_t face_id: m_visual->getFaces()) + { + if (m_faceSelectedArray.size() <= face_id) + { + continue; + } + m_faceSelectedArray[face_id] = true; + } + } + // Prepare for GPU accelerated selection updateSelectionMesh(); } @@ -526,14 +543,13 @@ void ClusterLabelTool::publishLabel(std::string label) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Tool: Publish label '" << label << "'"); - vector faces; - for (uint32_t i = 0; i < m_faceSelectedArray.size(); i++) + // Cannot save without having a marked visual + if (!m_visual) { - if (m_faceSelectedArray[i]) - faces.push_back(i); + return; } - m_display->addLabel(label, faces); + m_display->addLabel(label, m_visual->getFaces()); } // Handling mouse event and mark the clicked faces diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 45a8c56..32da0dc 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -811,8 +811,8 @@ bool MapDisplay::loadData() void MapDisplay::saveLabel(Cluster cluster) { - std::string label = cluster.name; - std::vector faces = cluster.faces; + const std::string& label = cluster.name; + const std::vector& faces = cluster.faces; RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: add label '" << label << "'"); @@ -842,8 +842,16 @@ void MapDisplay::saveLabel(Cluster cluster) std::copy(faces.begin(), faces.end(), faces_array.get()); hdf5_mesh_io->ArrayIO::save(label_group, results[1], faces.size(), faces_array); - // Add to cluster list - m_clusterList.push_back(Cluster(label, faces)); + // Add to cluster list or update existing cluster + auto it = std::find_if(m_clusterList.begin(), m_clusterList.end(), [label](const Cluster& c){ return label == c.name;}); + if (it != m_clusterList.end()) + { + *it = cluster; + } + else + { + m_clusterList.push_back(cluster); + } setStatus(rviz_common::properties::StatusProperty::Ok, "Label", "Successfully saved label"); RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Successfully added label to map."); From 25fe0d680fa6ecba809904548ac879a697faed4a Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Sat, 24 May 2025 15:14:46 +0200 Subject: [PATCH 09/12] Propagate the MeshDisplay Culling Mode to ClusterLabel[Tool|Visual|Display] --- .../ClusterLabelDisplay.hpp | 8 ++++++++ .../rviz_mesh_tools_plugins/ClusterLabelTool.hpp | 6 ++++++ .../ClusterLabelVisual.hpp | 7 ++++++- .../rviz_mesh_tools_plugins/MeshDisplay.hpp | 6 ++++++ .../src/ClusterLabelDisplay.cpp | 15 +++++++++++++++ rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp | 12 ++++++++++++ .../src/ClusterLabelVisual.cpp | 8 ++++++++ rviz_mesh_tools_plugins/src/MapDisplay.cpp | 2 ++ rviz_mesh_tools_plugins/src/MeshDisplay.cpp | 4 +++- 9 files changed, 66 insertions(+), 2 deletions(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp index 1e03ec8..4b26dba 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp @@ -198,6 +198,11 @@ public Q_SLOTS: // not sure wether any of those actually need to be q slots ... */ void setData(shared_ptr geometry, vector clusters); + /** + * @brief Slot for changing the culling mode used in the LabelTool, and the LabelVisuals + */ + void setCullingMode(Ogre::CullingMode mode); + private Q_SLOTS: /** @@ -293,6 +298,9 @@ private Q_SLOTS: /// A variable that will be set to true, once the initial data has arrived bool has_data = false; + /// Current Culling Mode + Ogre::CullingMode m_cullingMode; + }; diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 1ea1db0..8ed2aad 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -182,6 +182,11 @@ class ClusterLabelTool : public rviz_common::Tool */ void setBrushSize(float size); + /** + * @brief Set the culling mode for selection to match the MeshVisual + */ + void setCullingMode(Ogre::CullingMode mode); + public Q_SLOTS: /** @@ -271,6 +276,7 @@ public Q_SLOTS: Ogre::SceneNode* m_selectionSceneNode; // Used to render only the selectionMesh to the offscreen Texture uint32_t m_selectionVisibilityBit; + Ogre::CullingMode m_cullingMode; }; } // end namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp index a280ef7..3a11880 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp @@ -160,6 +160,11 @@ class ClusterLabelVisual return m_faces; }; + /** + * @brief Set the culling mode to match the MeshDisplay/MeshVisual + */ + void setCullingMode(Ogre::CullingMode mode); + private: void initMaterial(); @@ -179,4 +184,4 @@ class ClusterLabelVisual } // end namespace rviz_mesh_tools_plugins -#endif \ No newline at end of file +#endif diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp index c5f34a1..c053f45 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp @@ -246,6 +246,12 @@ class MeshDisplay : public rviz_common::Display */ void setPose(Ogre::Vector3& position, Ogre::Quaternion& orientation); +signals: + /** + * @brief Signals that the Culling Mode of the MeshDisplay was set. + */ + void signalCullingModeChanged(Ogre::CullingMode mode); + private Q_SLOTS: void transformerChangedCallback(); diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index a1662f4..275fb0b 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -97,6 +97,7 @@ ClusterLabelDisplay::ClusterLabelDisplay() , m_colorsProperty(nullptr) , m_sphereSizeProperty(nullptr) , m_phantomVisualProperty(nullptr) +, m_cullingMode(Ogre::CullingMode::CULL_NONE) { m_activeVisualProperty = new rviz_common::properties::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", @@ -161,6 +162,19 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); } +void ClusterLabelDisplay::setCullingMode(Ogre::CullingMode mode) +{ + auto* tool = getOrCreateLabelTool(); + if (tool) + { + tool->setCullingMode(mode); + } + for (auto& visual: m_visuals) + { + visual->setCullingMode(mode); + } +} + // ===================================================================================================================== // Callbacks @@ -329,6 +343,7 @@ void ClusterLabelDisplay::createVisualsFromClusterList() ); visual->setFacesInCluster(m_clusterList[i].faces); visual->setColor(getRainbowColor((++colorIndex / m_clusterList.size())), m_alphaProperty->getFloat()); + visual->setCullingMode(m_cullingMode); m_visuals.push_back(visual); } } diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 67f9bb3..260e10a 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -87,6 +87,7 @@ ClusterLabelTool::ClusterLabelTool() , m_selectionCircle(nullptr) , m_selectionMesh(nullptr) , m_selectionVisibilityBit(0) +, m_cullingMode(Ogre::CullingMode::CULL_NONE) { shortcut_key_ = 'l'; } @@ -200,6 +201,7 @@ void ClusterLabelTool::onInitialize() ); } + m_selectionMaterial->setCullingMode(m_cullingMode); Ogre::Technique* tech = m_selectionMaterial->getTechnique(0); Ogre::Pass* pass = tech->getPass(0); pass->setLightingEnabled(false); @@ -838,6 +840,16 @@ void ClusterLabelTool::initSelectionCircle() m_selectionCircle->end(); m_selectionCircle->setMaterial(0, m_selectionBoxMaterial); } + +void ClusterLabelTool::setCullingMode(Ogre::CullingMode mode) +{ + m_cullingMode = mode; + if (m_selectionMaterial) + { + m_selectionMaterial->setCullingMode(mode); + } +} + } // End namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index ddf95eb..659e0b0 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -291,6 +291,14 @@ void ClusterLabelVisual::setColor(Ogre::ColourValue facesColor, float alpha) } } +void ClusterLabelVisual::setCullingMode(Ogre::CullingMode mode) +{ + if (m_material) + { + m_material->setCullingMode(mode); + } +} + void ClusterLabelVisual::initMaterial() { m_material->setCullingMode(Ogre::CULL_NONE); diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 32da0dc..b299252 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -430,6 +430,8 @@ bool MapDisplay::loadData() { enableClusterLabelDisplay(); // enable label writing to hdf5 enableMeshDisplay(); + // The ClusterLabelDisplay shall be notified of CullingModeChanges in the MeshDisplay + QObject::connect(m_meshDisplay, &MeshDisplay::signalCullingModeChanged, m_clusterLabelDisplay, &ClusterLabelDisplay::setCullingMode); RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load HDF5 map"); diff --git a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp index 00aeb4e..f2a6156 100644 --- a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp @@ -858,6 +858,8 @@ void MeshDisplay::updateCullingMode() { visual->setCullingMode(static_cast(m_cullingMode->getOptionInt())); } + + emit signalCullingModeChanged(static_cast(m_cullingMode->getOptionInt())); } // ===================================================================================================================== @@ -1219,4 +1221,4 @@ std::shared_ptr MeshDisplay::addNewVisual() } // End namespace rviz_mesh_tools_plugins #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MeshDisplay, rviz_common::Display) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MeshDisplay, rviz_common::Display) From f22828ca8cbf613fa5103a78b9ac82873700620c Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Sat, 24 May 2025 16:38:55 +0200 Subject: [PATCH 10/12] Fix Reset Faces button would break selection --- rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 260e10a..8ebf54b 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -677,7 +677,7 @@ std::vector ClusterLabelTool::getSelectedFaces() void ClusterLabelTool::resetFaces() { - m_faceSelectedArray.clear(); + std::fill(m_faceSelectedArray.begin(), m_faceSelectedArray.end(), false); if(m_visual) { m_visual->setFacesInCluster(std::vector()); From 73114c72f1743dec34eef3dfa5f3bbb57e747e24 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Sun, 25 May 2025 11:30:31 +0200 Subject: [PATCH 11/12] Compatibility with ROS Humble --- .../ClusterLabelTool.hpp | 2 +- .../src/ClusterLabelTool.cpp | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 8ed2aad..38d6a08 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -217,7 +217,7 @@ public Q_SLOTS: ClusterLabelDisplay* m_display; std::shared_ptr m_visual; std::shared_ptr m_meshGeometry; - float m_brushSize = 1.0f; + float m_brushSize; // Selection Box Ogre::SceneNode* m_sceneNode; diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 8ebf54b..7d1bfb9 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -84,6 +84,7 @@ namespace rviz_mesh_tools_plugins ClusterLabelTool::ClusterLabelTool() :rviz_common::Tool() , m_display(nullptr) +, m_brushSize(MIN_BRUSH_SIZE) , m_selectionCircle(nullptr) , m_selectionMesh(nullptr) , m_selectionVisibilityBit(0) @@ -709,7 +710,25 @@ Ogre::Image ClusterLabelTool::renderMeshWithFaceID() render_texture->update(); // Copy the data to ram + /* NOTE: Since Ogre 1.12.10 (which is available with ROS Jazzy), Ogre::Image has a constructor + * which creates an Image from a given size and PixelFormat. + * This #if-macro is to keep supporting ROS Humble (which uses Ogre 1.12.1) and can + * be removed when support for ROS Humble is dropped. + */ +#if OGRE_VERSION < ((1 << 16) | (12 << 8) | (10)) + Ogre::Image img; + const size_t width = render_texture->getWidth(); + const size_t height = render_texture->getHeight(); + const size_t depth = 1; + Ogre::uchar* mem = OGRE_ALLOC_T( + Ogre::uchar, + Ogre::PixelUtil::getMemorySize(width, height, depth, Ogre::PF_R8G8B8), + Ogre::MEMCATEGORY_GENERAL + ); + img.loadDynamicImage(mem, width, height, depth, Ogre::PF_R8G8B8, true); +#else Ogre::Image img(Ogre::PF_R8G8B8, render_texture->getWidth(), render_texture->getHeight()); +#endif Ogre::PixelBox pixels = img.getPixelBox(); render_texture->copyContentsToMemory(pixels, pixels); From 5e8058cb619b454a429b8f41918ddb94181c8931 Mon Sep 17 00:00:00 2001 From: Justus Braun Date: Tue, 3 Jun 2025 12:28:47 +0200 Subject: [PATCH 12/12] Add nullptr check to MeshBuffer loaded from HDF5 file --- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index b299252..04bf9cd 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -331,8 +331,15 @@ void MapDisplay::updateMap() // Load geometry and clusters bool successful = loadData(); - if (!successful) + if (successful) { + enableClusterLabelDisplay(); + enableMeshDisplay(); + } + else + { + disableClusterLabelDisplay(); + disableMeshDisplay(); return; } @@ -426,7 +433,7 @@ bool MapDisplay::loadData() try { - if (boost::filesystem::extension(mapFile).compare(".h5") == 0) + if (boost::filesystem::path(mapFile).extension().compare(".h5") == 0) { enableClusterLabelDisplay(); // enable label writing to hdf5 enableMeshDisplay(); @@ -443,6 +450,16 @@ bool MapDisplay::loadData() hdf5_mesh_io->open(mapFile); auto mesh_buffer = hdf5_mesh_io->MeshIO::load(mesh_part); + if (nullptr == mesh_buffer) + { + RCLCPP_ERROR( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "Could not find a mesh with name '%s' in HDF5 file '%s'", + mesh_part.c_str(), mapFile.c_str() + ); + return false; + } + // the mesh buffer is a map from a string to a Channel // example: // "vertices" -> Channel