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/ClusterLabelDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp index 59d86ba..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: /** @@ -232,9 +237,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 +274,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; @@ -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 9741b00..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 @@ -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 @@ -136,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 */ @@ -181,10 +177,15 @@ 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 setBrushSize(float size); + + /** + * @brief Set the culling mode for selection to match the MeshVisual */ - void setSphereSize(float size); + void setCullingMode(Ogre::CullingMode mode); public Q_SLOTS: @@ -213,20 +214,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; // 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,50 +234,50 @@ 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 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); + 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; - - // 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; + /** + * @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; + Ogre::ManualObject* m_selectionMesh; + 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 -#endif \ No newline at end of file +#endif 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 e838685..275fb0b 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -92,6 +92,12 @@ Ogre::ColourValue getRainbowColor(float value) } ClusterLabelDisplay::ClusterLabelDisplay() +: m_activeVisualProperty(nullptr) +, m_alphaProperty(nullptr) +, 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", @@ -106,7 +112,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", @@ -156,13 +162,29 @@ 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 void ClusterLabelDisplay::onInitialize() { // Look for an existing label tool or create a new one - initializeLabelTool(); + ClusterLabelTool* tool = getOrCreateLabelTool(); + + // Set the Tool related configuration + tool->setBrushSize(m_sphereSizeProperty->getFloat()); } void ClusterLabelDisplay::onEnable() @@ -174,7 +196,11 @@ void ClusterLabelDisplay::onDisable() { m_visuals.clear(); m_phantomVisual.reset(); - m_tool->resetVisual(); + + if (auto* tool = getOrCreateLabelTool()) + { + tool->resetVisual(); + } } // ===================================================================================================================== @@ -206,12 +232,15 @@ 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!"); + return; } // Now create the visuals for the loaded clusters @@ -230,7 +259,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 +270,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 +279,10 @@ void ClusterLabelDisplay::updateColors() void ClusterLabelDisplay::updateSphereSize() { - m_tool->setSphereSize(m_sphereSizeProperty->getFloat()); + if (auto* tool = getOrCreateLabelTool()) + { + tool->setBrushSize(m_sphereSizeProperty->getFloat()); + } } void ClusterLabelDisplay::updatePhantomVisual() @@ -269,7 +304,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,15 +331,19 @@ 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; 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()); + visual->setCullingMode(m_cullingMode); m_visuals.push_back(visual); } } @@ -324,37 +363,53 @@ 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; } } + + 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..7d1bfb9 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -45,26 +45,20 @@ * */ +#include #include #include #include -#include - #include #include #include -#include -#include - -#include #include #include - #include - #include +#include #include @@ -72,6 +66,10 @@ #include #include #include +#include +#include +#include +#include #include @@ -82,15 +80,15 @@ 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_brushSize(MIN_BRUSH_SIZE) +, m_selectionCircle(nullptr) +, m_selectionMesh(nullptr) +, m_selectionVisibilityBit(0) +, m_cullingMode(Ogre::CullingMode::CULL_NONE) { shortcut_key_ = 'l'; } @@ -98,9 +96,42 @@ ClusterLabelTool::ClusterLabelTool() ClusterLabelTool::~ClusterLabelTool() { m_selectedFaces.clear(); - context_->getSceneManager()->destroyManualObject(m_selectionBox->getName()); - context_->getSceneManager()->destroyManualObject(m_selectionBoxMaterial->getName()); - context_->getSceneManager()->destroySceneNode(m_sceneNode); + 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); } // onInitialize() is called by the superclass after scene_manager_ and @@ -120,6 +151,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( @@ -138,131 +170,50 @@ void ClusterLabelTool::onInitialize() m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); + // 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 + 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 + ); + } + + // Create a material + 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 + ); + } - // // 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); - // } + m_selectionMaterial->setCullingMode(m_cullingMode); + 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) @@ -270,21 +221,24 @@ 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; } } -void ClusterLabelTool::setSphereSize(float size) +void ClusterLabelTool::setBrushSize(float size) { - // m_clKernelSphere.setArg(3, size); - m_sphereSize = size; + m_brushSize = size; } void ClusterLabelTool::activate() @@ -293,75 +247,36 @@ 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) { + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelTool::setDisplay()"); m_display = display; m_meshGeometry = m_display->getGeometry(); - m_faceSelectedArray.reserve(m_meshGeometry->faces.size()); - m_displayInitialized = true; - m_vertexData.reserve(m_meshGeometry->faces.size() * 3 * 3); + // Allocate a buffer and init to unselected + m_faceSelectedArray.resize(m_meshGeometry->faces.size()); + std::fill(m_faceSelectedArray.begin(), m_faceSelectedArray.end(), false); - for (uint32_t faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) + // 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 (uint32_t i = 0; i < 3; i++) + for (size_t face_id: m_visual->getFaces()) { - 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); + if (m_faceSelectedArray.size() <= face_id) + { + continue; + } + m_faceSelectedArray[face_id] = true; } } - // 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); - - // m_clKernelSingleRay = cl::Kernel(m_clProgram, "cast_rays"); - // m_clKernelSphere = cl::Kernel(m_clProgram, "cast_sphere"); - // m_clKernelBox = cl::Kernel(m_clProgram, "cast_box"); - - // m_clKernelSingleRay.setArg(0, m_clVertexBuffer); - // m_clKernelSingleRay.setArg(1, m_clRayBuffer); - // m_clKernelSingleRay.setArg(2, m_clResultBuffer); - - // m_clKernelSphere.setArg(0, m_clVertexBuffer); - // m_clKernelSphere.setArg(1, m_clSphereBuffer); - // m_clKernelSphere.setArg(2, m_clResultBuffer); - // m_clKernelSphere.setArg(3, m_sphereSize); - - // 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); - // } + // Prepare for GPU accelerated selection + updateSelectionMesh(); } void ClusterLabelTool::updateSelectionBox() @@ -389,10 +304,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(); @@ -402,14 +317,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; @@ -422,14 +329,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; @@ -441,126 +340,70 @@ void ClusterLabelTool::selectMultipleFaces( rviz_common::ViewportMouseEvent& event, bool selectMode) { - m_selectionBox->setVisible(false); - - int left = m_bb_x1; - int right = m_bb_x2; - int top = m_bb_y1; - int bottom = m_bb_y2; - - size_t goalSection; - size_t goalIndex; - - if (left > right) + (void) event; + // No map loaded or ClusterLabel display is not active + if (!m_visual) { - std::swap(left, right); + return; } - 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; + Ogre::Image img = renderMeshWithFaceID(); - auto manager = context_->getSelectionManager(); + // 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); - manager->pick( - m_evt_panel->getRenderWindow(), - m_bb_x1, m_bb_y1, - m_bb_x2, m_bb_y2, - pick_results); + 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); - if(!pick_results.empty()) - { - // FOUND SOMETHING! - // std::cout << "FOUND SOMETHING! " << pick_results.size() << std::endl; + RCLCPP_DEBUG( + rclcpp::get_logger("rviz_mesh_tools_plugins"), + "Selected rect uv: (%f, %f) (%f, %f)", + u_min, v_min, u_max, v_max + ); - for(auto elem : pick_results) - { - 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; + // 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"), + "Selected rect tex: (%u, %u) (%u, %u)", + tex_x0, tex_y0, tex_x1, tex_y1 + ); - // TODO: continue implementing this - } else { - // std::cout << "NOTHING to pick :(" << std::endl; - } - } - - // 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; - - -} - -void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode) -{ - m_boxData.clear(); - for (Ogre::Plane plane : volume.planes) + // Update all faces in the selected region + for (uint32_t y = tex_y0; y < tex_y1; y++) { - 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) + for (uint32_t x = tex_x0; x < tex_x1; x++) { - if (m_faceSelectedArray.size() <= faceId) + Ogre::ColourValue color = img.getColourAt(x, y, 0); + color.a = 0.0; + uint32_t face_id = color.getAsARGB(); + if (face_id >= m_faceSelectedArray.size()) { - m_faceSelectedArray.resize(faceId + 1); + // These are pixels which have the background color + continue; + } + else + { + m_faceSelectedArray[face_id] = selectMode; } - m_faceSelectedArray[faceId] = selectMode; } } + // 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 (uint32_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + for(size_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) { if (m_faceSelectedArray[faceId]) { @@ -568,13 +411,14 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume } } - if (m_displayInitialized && m_visual) + // Maybe the user has not loaded a map yet? + if (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, bool selectMode) @@ -587,7 +431,7 @@ void ClusterLabelTool::selectSingleFace( { // std::cout << "selectSingleFace- HIT!" << std::endl; - if (m_displayInitialized && m_visual) + if (m_display && m_visual) { std::vector tmpFaceList; @@ -598,7 +442,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]) { @@ -615,254 +459,204 @@ 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++) +void ClusterLabelTool::selectCircleFaces( + rviz_common::ViewportMouseEvent& event, bool selectMode) +{ + // No map loaded or ClusterLabel display is not active + if (!m_visual) { - if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) - { - closestFaceId = i; - minDist = m_resultDistances[i]; - } + return; } - if (m_displayInitialized && m_visual && closestFaceId != -1) + // 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++) { - std::vector tmpFaceList; - - if (m_faceSelectedArray.size() <= closestFaceId) + for (uint32_t x = x_min; x < x_max; x++) { - m_faceSelectedArray.resize(closestFaceId + 1); - } - m_faceSelectedArray[closestFaceId] = selectMode; + // Check if the point lies inside the img space ellipse + const int32_t dx = x - cx; + const int32_t dy = y - cy; - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) + const float t = (std::pow(dx, 2) / std::pow(rx, 2)) + (std::pow(dy, 2) / std::pow(ry, 2)); + if (t >= 1.0) { - tmpFaceList.push_back(faceId); + continue; } - } - 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; - } - } + // Point is inside the ellipse + Ogre::ColourValue color = buffer.getColourAt(x, y, 0); + color.a = 0.0; + const uint32_t face_id = color.getAsARGB(); - if (m_displayInitialized && m_visual) - { - std::vector tmpFaceList; - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + // Background color is white and results in an invalid face_id + if (face_id >= m_faceSelectedArray.size()) { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } + continue; } - m_visual->setFacesInCluster(tmpFaceList); + m_faceSelectedArray[face_id] = selectMode; } } -} -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) + // Update the visual + std::vector tmpFaceList; + for(size_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) { - closestFaceId = i; - faceFound = true; - minDist = m_resultDistances[i]; + tmpFaceList.push_back(faceId); } } - if (faceFound) - { - return std::make_pair(closestFaceId, minDist); - } - else - { - return {}; - } + m_visual->setFacesInCluster(tmpFaceList); } + 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 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 + */ + + // Show circle overlay when control is pressed + if (event.control()) + { + // Update the brush size with the 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); + } + 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; - selectSphereFaces(event, true); + selectSingleFace(event, true); } else if (event.leftUp() && m_singleSelect) { m_singleSelect = false; - selectSphereFaces(event, true); } - else if (event.rightDown() && event.control()) + else if (event.leftUp() && m_multipleSelect) { - m_singleDeselect = true; - selectSphereFaces(event, false); + m_multipleSelect = false; + m_selectionBox->setVisible(false); + selectMultipleFaces(event, true); } - else if (event.rightUp() && m_singleDeselect) + else if (event.leftUp() && m_circleSelect) { - m_singleDeselect = false; - selectSphereFaces(event, false); + m_circleSelect = false; } - 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() && event.control()) { - m_multipleSelect = false; - selectMultipleFaces(event, true); + m_circleSelect = true; + m_selectionMode = false; + selectCircleFaces(event, false); } else if (event.rightDown()) { - m_multipleSelect = true; - selectionBoxStart(event); + m_singleSelect = true; + selectSingleFace(event, false); + } + else if (event.rightUp() && m_singleSelect) + { + m_singleSelect = false; } else if (event.rightUp() && m_multipleSelect) { m_multipleSelect = false; + m_selectionBox->setVisible(false); selectMultipleFaces(event, false); } - else if (m_multipleSelect) + else if (event.rightUp() && m_circleSelect) { - selectionBoxMove(event); + m_circleSelect = false; } - else if (m_singleSelect) + else if (m_multipleSelect) { - selectSphereFaces(event, true); + selectionBoxMove(event); } - else if (m_singleDeselect) + else if (m_circleSelect) { - selectSphereFaces(event, false); + selectCircleFaces(event, m_selectionMode); } return Render; @@ -872,7 +666,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]) { @@ -884,7 +678,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()); @@ -893,13 +687,190 @@ void ClusterLabelTool::resetFaces() void ClusterLabelTool::resetVisual() { - // TODO: Segfault here, when using RViz config m_visual.reset(); } -} // End namespace rviz_mesh_tools_plugins +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 + /* 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); + + // 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) + { + // Init the ManualObject used for offscreen rendering + m_selectionMesh = scene_manager_->createManualObject("ClusterLabelToolPickingMesh"); + 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()) + { + 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() + ); +} + +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); +} + +void ClusterLabelTool::setCullingMode(Ogre::CullingMode mode) +{ + m_cullingMode = mode; + if (m_selectionMaterial) + { + m_selectionMaterial->setCullingMode(mode); + } +} + +} // End namespace rviz_mesh_tools_plugins + #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) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index 2b5c17f..659e0b0 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); } @@ -290,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 b8dd527..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,10 +433,12 @@ 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(); + // 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"); @@ -441,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 @@ -546,6 +565,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 { @@ -771,8 +830,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 << "'"); @@ -788,15 +847,30 @@ 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)); + // 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."); 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) 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); }