From 98c249b099a19e58e92c42a8c9b8f7bc485c9a0a Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 16 May 2023 11:58:42 +0200 Subject: [PATCH 01/40] added assimp --- rviz_map_plugin/CMakeLists.txt | 8 + rviz_map_plugin/src/MapDisplay.cpp | 258 +++++++++++++++++------------ 2 files changed, 160 insertions(+), 106 deletions(-) diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index 3fc63da..90b90fa 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -17,6 +17,14 @@ find_package(Boost REQUIRED COMPONENTS system) find_package(HDF5 REQUIRED COMPONENTS C CXX HL) find_package(OpenCL 2 REQUIRED) +find_package(assimp) + +if(assimp_FOUND) + include_directories(${ASSIMP_INCLUDE_DIRS}) + add_definitions(-DWITH_ASSIMP) +endif(assimp_FOUND) + + catkin_package( CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} DEPENDS Boost OpenCL HDF5 OpenCL diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index d6e40d6..93ba8cc 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -60,6 +60,12 @@ #include #include +#if defined(WITH_ASSIMP) + #include + #include + #include +#endif + namespace rviz_map_plugin { MapDisplay::MapDisplay() @@ -191,141 +197,181 @@ bool MapDisplay::loadData() setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file does not exist!"); return false; } - if (boost::filesystem::extension(mapFile).compare(".h5") != 0) - { - ROS_WARN_STREAM("Map Display: Specified map file is not a .h5 file!"); - setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file is not a .h5 file!"); - return false; - } + ROS_INFO_STREAM("Map Display: Loading data for map '" << mapFile << "'"); try { - // Open file IO - hdf5_map_io::HDF5MapIO map_io(mapFile); + if (boost::filesystem::extension(mapFile).compare(".h5") == 0) + { + ROS_INFO("Map Display: Load HDF5 map"); + // Open file IO + hdf5_map_io::HDF5MapIO map_io(mapFile); - ROS_INFO("Map Display: Load geometry"); + ROS_INFO("Map Display: Load geometry"); - // Read geometry - m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); + // Read geometry + m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); - ROS_INFO("Map Display: Load textures"); + ROS_INFO("Map Display: Load textures"); - // Read textures - vector textures = map_io.getTextures(); - m_textures.resize(textures.size()); - for (size_t i = 0; i < textures.size(); i++) - { - // Find out the texture index because textures are not stored in ascending order - int textureIndex = std::stoi(textures[i].name); - - // Copy metadata - m_textures[textureIndex].width = textures[i].width; - m_textures[textureIndex].height = textures[i].height; - m_textures[textureIndex].channels = textures[i].channels; - m_textures[textureIndex].data = textures[i].data; - m_textures[textureIndex].pixelFormat = "rgb8"; - } + // Read textures + vector textures = map_io.getTextures(); + m_textures.resize(textures.size()); + for (size_t i = 0; i < textures.size(); i++) + { + // Find out the texture index because textures are not stored in ascending order + int textureIndex = std::stoi(textures[i].name); + + // Copy metadata + m_textures[textureIndex].width = textures[i].width; + m_textures[textureIndex].height = textures[i].height; + m_textures[textureIndex].channels = textures[i].channels; + m_textures[textureIndex].data = textures[i].data; + m_textures[textureIndex].pixelFormat = "rgb8"; + } - ROS_INFO("Map Display: Load materials"); + ROS_INFO("Map Display: Load materials"); - // Read materials - vector materials = map_io.getMaterials(); - vector faceToMaterialIndexArray = map_io.getMaterialFaceIndices(); - m_materials.resize(materials.size()); - for (size_t i = 0; i < materials.size(); i++) - { - // Copy material color - m_materials[i].color.r = materials[i].r / 255.0f; - m_materials[i].color.g = materials[i].g / 255.0f; - m_materials[i].color.b = materials[i].b / 255.0f; - m_materials[i].color.a = 1.0f; - - // Look for texture index - if (materials[i].textureIndex == -1) + // Read materials + vector materials = map_io.getMaterials(); + vector faceToMaterialIndexArray = map_io.getMaterialFaceIndices(); + m_materials.resize(materials.size()); + for (size_t i = 0; i < materials.size(); i++) { - // texture index -1: no texture - m_materials[i].textureIndex = boost::none; + // Copy material color + m_materials[i].color.r = materials[i].r / 255.0f; + m_materials[i].color.g = materials[i].g / 255.0f; + m_materials[i].color.b = materials[i].b / 255.0f; + m_materials[i].color.a = 1.0f; + + // Look for texture index + if (materials[i].textureIndex == -1) + { + // texture index -1: no texture + m_materials[i].textureIndex = boost::none; + } + else + { + m_materials[i].textureIndex = materials[i].textureIndex; + } + + m_materials[i].faceIndices.clear(); } - else + + // Copy face indices + for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) { - m_materials[i].textureIndex = materials[i].textureIndex; + m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); } - m_materials[i].faceIndices.clear(); - } - - // Copy face indices - for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) - { - m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); - } - - ROS_INFO("Map Display: Load vertex colors"); + ROS_INFO("Map Display: Load vertex colors"); - // Read vertex colors - vector colors = map_io.getVertexColors(); - m_colors.clear(); - m_colors.reserve(colors.size() / 3); - for (size_t i = 0; i < colors.size(); i += 3) - { - // convert from 0-255 (uint8) to 0.0-1.0 (float) - m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); - } + // Read vertex colors + vector colors = map_io.getVertexColors(); + m_colors.clear(); + m_colors.reserve(colors.size() / 3); + for (size_t i = 0; i < colors.size(); i += 3) + { + // convert from 0-255 (uint8) to 0.0-1.0 (float) + m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); + } - ROS_INFO("Map Display: Load vertex normals"); + ROS_INFO("Map Display: Load vertex normals"); - // Read vertex normals - vector normals = map_io.getVertexNormals(); - m_normals.clear(); - m_normals.reserve(normals.size() / 3); - for (size_t i = 0; i < normals.size(); i += 3) - { - m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); - } + // Read vertex normals + vector normals = map_io.getVertexNormals(); + m_normals.clear(); + m_normals.reserve(normals.size() / 3); + for (size_t i = 0; i < normals.size(); i += 3) + { + m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); + } - ROS_INFO("Map Display: Load texture coordinates"); + ROS_INFO("Map Display: Load texture coordinates"); - // Read tex cords - vector texCoords = map_io.getVertexTextureCoords(); - m_texCoords.clear(); - m_texCoords.reserve(texCoords.size() / 3); - for (size_t i = 0; i < texCoords.size(); i += 3) - { - m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); - } + // Read tex cords + vector texCoords = map_io.getVertexTextureCoords(); + m_texCoords.clear(); + m_texCoords.reserve(texCoords.size() / 3); + for (size_t i = 0; i < texCoords.size(); i += 3) + { + m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); + } - ROS_INFO("Map Display: Load clusters"); + ROS_INFO("Map Display: Load clusters"); - // Read labels - m_clusterList.clear(); - // m_clusterList.push_back(Cluster("__NEW__", vector())); - for (auto labelGroup : map_io.getLabelGroups()) - { - for (auto labelObj : map_io.getAllLabelsOfGroup(labelGroup)) + // Read labels + m_clusterList.clear(); + // m_clusterList.push_back(Cluster("__NEW__", vector())); + for (auto labelGroup : map_io.getLabelGroups()) { - auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); + for (auto labelObj : map_io.getAllLabelsOfGroup(labelGroup)) + { + auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); - std::stringstream ss; - ss << labelGroup << "_" << labelObj; - std::string label = ss.str(); + std::stringstream ss; + ss << labelGroup << "_" << labelObj; + std::string label = ss.str(); - m_clusterList.push_back(Cluster(label, faceIds)); + m_clusterList.push_back(Cluster(label, faceIds)); + } } - } - m_costs.clear(); - for (std::string costlayer : map_io.getCostLayers()) + m_costs.clear(); + for (std::string costlayer : map_io.getCostLayers()) + { + try + { + m_costs[costlayer] = map_io.getVertexCosts(costlayer); + } + catch (const hf::DataSpaceException& e) + { + ROS_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); + } + } + } + #if defined(WITH_ASSIMP) + else { - try - { - m_costs[costlayer] = map_io.getVertexCosts(costlayer); - } - catch (const hf::DataSpaceException& e) - { - ROS_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); - } + // PLY, OBJ, DAE? -> ASSIMP + // The following lines are a simple way to import the mesh geometry + // of commonly used mesh file formats. + // + // TODOs: + // 1. scene graphs will not be imported properly. + // Someone has to do some transformations according to the + // node graph in the assimp structures. Or optionally (even better): + // create tf-transformations for every element of the scene graph + // + Assimp::Importer io; + io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); + const aiScene* ascene = io.ReadFile(mapFile, 0); + const aiMesh* amesh = ascene->mMeshes[0]; + + const aiVector3D* ai_vertices = amesh->mVertices; + const aiFace* ai_faces = amesh->mFaces; + + m_geometry = std::make_shared(); + + m_geometry->vertices.resize(amesh->mNumVertices); + m_geometry->faces.resize(amesh->mNumFaces); + + for (int i = 0; i < amesh->mNumVertices; i++) + { + m_geometry->vertices[i].x = amesh->mVertices[i].x; + m_geometry->vertices[i].y = amesh->mVertices[i].y; + m_geometry->vertices[i].z = amesh->mVertices[i].z; + } + + for (int i = 0; i < amesh->mNumFaces; i++) + { + m_geometry->faces[i].vertexIndices[0] = amesh->mFaces[i].mIndices[0]; + m_geometry->faces[i].vertexIndices[1] = amesh->mFaces[i].mIndices[1]; + m_geometry->faces[i].vertexIndices[2] = amesh->mFaces[i].mIndices[2]; + } } + #endif // defined(WITH_ASSIMP) } catch (...) { From 1ff1a3e2a246f0c5ee8bc4f037248f0c4c0f4bb0 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 16 May 2023 16:05:06 +0200 Subject: [PATCH 02/40] integrated assimp transforms for scaled, moved, or rotated meshes --- rviz_map_plugin/src/MapDisplay.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index 93ba8cc..4017cc7 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -346,7 +346,12 @@ bool MapDisplay::loadData() // Assimp::Importer io; io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); - const aiScene* ascene = io.ReadFile(mapFile, 0); + + // with aiProcess_PreTransformVertices assimp transforms the whole scene graph + // into one mesh + // - if you want to use TF for spawning meshes, the loading has to be done manually + const aiScene* ascene = io.ReadFile(mapFile, aiProcess_PreTransformVertices); + // what if there is more than one mesh? const aiMesh* amesh = ascene->mMeshes[0]; const aiVector3D* ai_vertices = amesh->mVertices; From a84506efaf999c39da42b7360d6ac1e2652c5a87 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 16 May 2023 16:23:55 +0200 Subject: [PATCH 03/40] added file filters for common mesh file types once assimp is available --- rviz_map_plugin/src/RvizFileProperty.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rviz_map_plugin/src/RvizFileProperty.cpp b/rviz_map_plugin/src/RvizFileProperty.cpp index a9c32b1..94afdca 100644 --- a/rviz_map_plugin/src/RvizFileProperty.cpp +++ b/rviz_map_plugin/src/RvizFileProperty.cpp @@ -63,6 +63,16 @@ QWidget* FileProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& QStringList filenameFilters; filenameFilters << tr("*.h5"); + #if defined(WITH_ASSIMP) + filenameFilters << tr("*.ply"); + filenameFilters << tr("*.obj"); + filenameFilters << tr("*.dae"); + filenameFilters << tr("*.stl"); + filenameFilters << tr("*.3d"); + filenameFilters << tr("*.3ds"); + filenameFilters << tr("*.fbx"); + filenameFilters << tr("*.blend"); + #endif filenameFilters << tr("*"); editor->setNameFilters(filenameFilters); From 1823ab894e0192354ac319deb25d77bfa30c4489 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 7 Sep 2023 13:53:09 +0200 Subject: [PATCH 04/40] added functionality to override rviz config with ros params --- rviz_map_plugin/include/MapDisplay.hpp | 8 ++++ rviz_map_plugin/src/MapDisplay.cpp | 64 ++++++++++++++++++++++++-- rviz_map_plugin/src/MeshDisplay.cpp | 2 +- 3 files changed, 70 insertions(+), 4 deletions(-) diff --git a/rviz_map_plugin/include/MapDisplay.hpp b/rviz_map_plugin/include/MapDisplay.hpp index db9cc96..83dab27 100644 --- a/rviz_map_plugin/include/MapDisplay.hpp +++ b/rviz_map_plugin/include/MapDisplay.hpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include @@ -78,6 +79,7 @@ #include #include #include +#include #include #include @@ -149,6 +151,8 @@ class MapDisplay : public rviz::Display */ ~MapDisplay(); + virtual void load(const rviz::Config& config); + public Q_SLOTS: /** @@ -210,8 +214,12 @@ private Q_SLOTS: std::map> m_costs; + std::shared_ptr m_nh; + std::shared_ptr m_nh_p; + /// Path to map file rviz::FileProperty* m_mapFilePath; + std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) rviz_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index 4017cc7..bbd3f2d 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -107,8 +107,13 @@ rviz::Display* MapDisplay::createDisplay(const QString& class_id) void MapDisplay::onInitialize() { + std::string name = this->getName().toStdString(); + Display* display = createDisplay("rviz_map_plugin/ClusterLabel"); + m_nh = std::make_shared("~"); + m_nh_p = std::make_shared("~"); + m_clusterLabelDisplay = static_cast(display); m_clusterLabelDisplay->setName("ClusterLabel"); m_clusterLabelDisplay->setModel(model_); @@ -128,8 +133,6 @@ void MapDisplay::onInitialize() // Make signal/slot connections connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); - - updateMap(); } void MapDisplay::onEnable() @@ -147,14 +150,42 @@ void MapDisplay::onDisable() // ===================================================================================================================== // Callbacks triggered from UI events (mostly) +void MapDisplay::load(const rviz::Config& config) +{ + std::string name = this->getName().toStdString(); + std::cout << name << ": LOAD CONFIG..." << std::endl; + + rviz::Config config2 = config; + + { // Override with ros params + std::stringstream ss; + ss << "rviz_map_plugin/" << name; + + std::string mesh_file; + if(m_nh_p->getParam(ss.str(), mesh_file)) + { + config2.mapSetValue(m_mapFilePath->getName(), QString::fromStdString(mesh_file) ); + } else { + std::cout << name << ": COULDN'T FOUND MESH TO LOAD" << std::endl; + } + } + + rviz::Display::load(config2); + + std::cout << name << ": LOAD CONFIG done." << std::endl; +} + void MapDisplay::updateMap() { - ROS_INFO("Map Display: Update"); + std::string name = this->getName().toStdString(); + std::cout << name << ": updateMap" << std::endl; // Load geometry and clusters bool successful = loadData(); if (!successful) + { return; + } // Update sub-plugins m_meshDisplay->setGeometry(m_geometry); @@ -176,6 +207,8 @@ void MapDisplay::updateMap() // All good setStatus(rviz::StatusProperty::Ok, "Map", ""); + + m_map_file_loaded = m_mapFilePath->getFilename(); } // ===================================================================================================================== @@ -183,6 +216,30 @@ void MapDisplay::updateMap() bool MapDisplay::loadData() { + + + std::string name = this->getName().toStdString(); + // std::stringstream ss; + // ss << "rviz_map_plugin/" << name; + + // std::string mesh_file; + // if(m_nh_p->getParam(ss.str(), mesh_file)) + // { + // std::cout<< name << ": FOUND INITIAL MESH IN PARAMS - " << mesh_file << std::endl; + // m_mapFilePath->setFilename(QString::fromStdString(mesh_file)); + // } else { + // std::cout << name << ": COULDN'T FOUND MESH TO LOAD" << std::endl; + // } + + + if(m_mapFilePath->getFilename() == m_map_file_loaded) + { + std::cout << name << "! Tried to load same map twice. Skipping and keeping old data" << std::endl; + return true; + } + + + // Read map file path std::string mapFile = m_mapFilePath->getFilename(); if (mapFile.empty()) @@ -334,6 +391,7 @@ bool MapDisplay::loadData() #if defined(WITH_ASSIMP) else { + std::cout << "LOADING WITH ASSIMP" << std::endl; // PLY, OBJ, DAE? -> ASSIMP // The following lines are a simple way to import the mesh geometry // of commonly used mesh file formats. diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index f6ea39f..00872d0 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -77,7 +77,7 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized - m_bufferSize = new rviz::IntProperty("Buffer Size", 1, "Amount of meshes visualized", this, SLOT(updateBufferSize())); + m_bufferSize = new rviz::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); m_bufferSize->setMin(1); // Display Type From 72881ee7e88d4ade2a3cdb59b4d8e12fd975f892 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 6 Nov 2023 17:28:18 +0100 Subject: [PATCH 05/40] ported hdf5_map_io to ros2 humple. COLCON_IGNORE are TODOs. --- hdf5_map_io/CMakeLists.txt | 30 ++++++++++++++++++----------- label_manager/COLCON_IGNORE | 0 mesh_msgs/COLCON_IGNORE | 0 mesh_msgs_conversions/COLCON_IGNORE | 0 mesh_msgs_hdf5/COLCON_IGNORE | 0 mesh_msgs_transform/COLCON_IGNORE | 0 mesh_tools/CMakeLists.txt | 8 ++++---- mesh_tools/package.xml | 19 ++++++++++-------- rviz_map_plugin/COLCON_IGNORE | 0 9 files changed, 34 insertions(+), 23 deletions(-) create mode 100644 label_manager/COLCON_IGNORE create mode 100644 mesh_msgs/COLCON_IGNORE create mode 100644 mesh_msgs_conversions/COLCON_IGNORE create mode 100644 mesh_msgs_hdf5/COLCON_IGNORE create mode 100644 mesh_msgs_transform/COLCON_IGNORE create mode 100644 rviz_map_plugin/COLCON_IGNORE diff --git a/hdf5_map_io/CMakeLists.txt b/hdf5_map_io/CMakeLists.txt index 746fadf..5e02edc 100644 --- a/hdf5_map_io/CMakeLists.txt +++ b/hdf5_map_io/CMakeLists.txt @@ -1,18 +1,25 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.8) project(hdf5_map_io) set(CMAKE_CXX_STANDARD 14) -find_package(catkin REQUIRED) +# DEFAULT RELEASE +if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) + if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) + endif() +endif() + +find_package(ament_cmake REQUIRED) find_package(LVR2 REQUIRED) find_package(MPI) find_package(PkgConfig REQUIRED) -catkin_package( - INCLUDE_DIRS include - LIBRARIES hdf5_map_io - DEPENDS LVR2 MPI -) +# catkin_package( +# INCLUDE_DIRS include +# LIBRARIES hdf5_map_io +# DEPENDS LVR2 MPI +# ) # HighFive set(HIGHFIVE_EXAMPLES FALSE) @@ -35,14 +42,15 @@ target_link_libraries(${PROJECT_NAME} ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE ) +ament_package() \ No newline at end of file diff --git a/label_manager/COLCON_IGNORE b/label_manager/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_msgs/COLCON_IGNORE b/mesh_msgs/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_msgs_conversions/COLCON_IGNORE b/mesh_msgs_conversions/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_msgs_hdf5/COLCON_IGNORE b/mesh_msgs_hdf5/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_msgs_transform/COLCON_IGNORE b/mesh_msgs_transform/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mesh_tools/CMakeLists.txt b/mesh_tools/CMakeLists.txt index 2111670..174d70b 100644 --- a/mesh_tools/CMakeLists.txt +++ b/mesh_tools/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mesh_tools) -find_package(catkin REQUIRED) -catkin_metapackage() +cmake_minimum_required(VERSION 3.8) +project(mesh_tools NONE) +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/mesh_tools/package.xml b/mesh_tools/package.xml index 652e2e0..894c281 100644 --- a/mesh_tools/package.xml +++ b/mesh_tools/package.xml @@ -1,5 +1,6 @@ - + + mesh_tools 1.1.0 The mesh_tools package @@ -8,15 +9,17 @@ http://wiki.ros.org/mesh_tools Sebastian Pütz + ament_cmake + hdf5_map_io - mesh_msgs - mesh_msgs_transform - mesh_msgs_conversions - rviz_map_plugin - label_manager - catkin + + + + + + - + ament_cmake diff --git a/rviz_map_plugin/COLCON_IGNORE b/rviz_map_plugin/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 From aee249ce990b402b2e95df2d63708e067904ddcd Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 6 Nov 2023 17:39:32 +0100 Subject: [PATCH 06/40] ament_cmake to package xml --- hdf5_map_io/package.xml | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/hdf5_map_io/package.xml b/hdf5_map_io/package.xml index 142d203..535e5e6 100644 --- a/hdf5_map_io/package.xml +++ b/hdf5_map_io/package.xml @@ -1,5 +1,6 @@ - + + hdf5_map_io 1.1.0 The hdf5_map_io package @@ -8,8 +9,14 @@ http://wiki.ros.org/ros_mesh_tools/hdf5_map_io Sebastian Pütz - catkin + ament_cmake boost lvr2 + ament_lint_auto + ament_lint_common + + + ament_cmake + From 9070fe54b8281b9eb6e630ec2e1a92df7cf615aa Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 7 Nov 2023 01:06:28 +0100 Subject: [PATCH 07/40] ported mesh_msgs to ros2 humble --- mesh_msgs/CMakeLists.txt | 97 +++++++++---------- mesh_msgs/COLCON_IGNORE | 0 mesh_msgs/package.xml | 30 +++--- mesh_msgs/{service => srv}/GetGeometry.srv | 0 .../{service => srv}/GetLabeledClusters.srv | 0 mesh_msgs/{service => srv}/GetMaterials.srv | 0 mesh_msgs/{service => srv}/GetTexture.srv | 0 mesh_msgs/{service => srv}/GetUUIDs.srv | 0 .../{service => srv}/GetVertexColors.srv | 0 .../{service => srv}/GetVertexCostLayers.srv | 0 mesh_msgs/{service => srv}/GetVertexCosts.srv | 0 11 files changed, 62 insertions(+), 65 deletions(-) delete mode 100644 mesh_msgs/COLCON_IGNORE rename mesh_msgs/{service => srv}/GetGeometry.srv (100%) rename mesh_msgs/{service => srv}/GetLabeledClusters.srv (100%) rename mesh_msgs/{service => srv}/GetMaterials.srv (100%) rename mesh_msgs/{service => srv}/GetTexture.srv (100%) rename mesh_msgs/{service => srv}/GetUUIDs.srv (100%) rename mesh_msgs/{service => srv}/GetVertexColors.srv (100%) rename mesh_msgs/{service => srv}/GetVertexCostLayers.srv (100%) rename mesh_msgs/{service => srv}/GetVertexCosts.srv (100%) diff --git a/mesh_msgs/CMakeLists.txt b/mesh_msgs/CMakeLists.txt index f43942d..f5eda9b 100644 --- a/mesh_msgs/CMakeLists.txt +++ b/mesh_msgs/CMakeLists.txt @@ -1,61 +1,54 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(mesh_msgs) -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - message_generation - roscpp - sensor_msgs - std_msgs -) -add_message_files( - FILES - MeshFaceCluster.msg - MeshFaceClusterStamped.msg - MeshMaterial.msg - MeshGeometry.msg - MeshGeometryStamped.msg - MeshMaterials.msg - MeshMaterialsStamped.msg - MeshVertexColors.msg - MeshVertexColorsStamped.msg - MeshVertexCosts.msg - MeshVertexCostsStamped.msg - MeshTexture.msg - MeshTriangleIndices.msg - VectorField.msg - VectorFieldStamped.msg - MeshVertexTexCoords.msg -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -add_service_files( - DIRECTORY - service - FILES - GetGeometry.srv - GetLabeledClusters.srv - GetMaterials.srv - GetTexture.srv - GetUUIDs.srv - GetVertexColors.srv - GetVertexCosts.srv - GetVertexCostLayers.srv -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) -generate_messages( - DEPENDENCIES - geometry_msgs - sensor_msgs - std_msgs -) -catkin_package( - INCLUDE_DIRS - CATKIN_DEPENDS +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MeshFaceCluster.msg" + "msg/MeshFaceClusterStamped.msg" + "msg/MeshMaterial.msg" + "msg/MeshGeometry.msg" + "msg/MeshGeometryStamped.msg" + "msg/MeshMaterials.msg" + "msg/MeshMaterialsStamped.msg" + "msg/MeshVertexColors.msg" + "msg/MeshVertexColorsStamped.msg" + "msg/MeshVertexCosts.msg" + "msg/MeshVertexCostsStamped.msg" + "msg/MeshTexture.msg" + "msg/MeshTriangleIndices.msg" + "msg/VectorField.msg" + "msg/VectorFieldStamped.msg" + "msg/MeshVertexTexCoords.msg" + "srv/GetGeometry.srv" + "srv/GetLabeledClusters.srv" + "srv/GetMaterials.srv" + "srv/GetTexture.srv" + "srv/GetUUIDs.srv" + "srv/GetVertexColors.srv" + "srv/GetVertexCosts.srv" + "srv/GetVertexCostLayers.srv" + DEPENDENCIES + std_msgs geometry_msgs - message_runtime - roscpp sensor_msgs - std_msgs + ADD_LINTER_TESTS ) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() \ No newline at end of file diff --git a/mesh_msgs/COLCON_IGNORE b/mesh_msgs/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mesh_msgs/package.xml b/mesh_msgs/package.xml index 6a824ff..0e8e280 100644 --- a/mesh_msgs/package.xml +++ b/mesh_msgs/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs 1.1.0 Various Message Types for Mesh Data. @@ -13,17 +14,20 @@ http://wiki.ros.org/mesh_tools/mesh_msgs BSD-3 - catkin + ament_cmake + rosidl_default_generators - roscpp - message_generation - geometry_msgs - sensor_msgs - std_msgs - - roscpp - message_runtime - geometry_msgs - sensor_msgs - std_msgs + geometry_msgs + sensor_msgs + std_msgs + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + diff --git a/mesh_msgs/service/GetGeometry.srv b/mesh_msgs/srv/GetGeometry.srv similarity index 100% rename from mesh_msgs/service/GetGeometry.srv rename to mesh_msgs/srv/GetGeometry.srv diff --git a/mesh_msgs/service/GetLabeledClusters.srv b/mesh_msgs/srv/GetLabeledClusters.srv similarity index 100% rename from mesh_msgs/service/GetLabeledClusters.srv rename to mesh_msgs/srv/GetLabeledClusters.srv diff --git a/mesh_msgs/service/GetMaterials.srv b/mesh_msgs/srv/GetMaterials.srv similarity index 100% rename from mesh_msgs/service/GetMaterials.srv rename to mesh_msgs/srv/GetMaterials.srv diff --git a/mesh_msgs/service/GetTexture.srv b/mesh_msgs/srv/GetTexture.srv similarity index 100% rename from mesh_msgs/service/GetTexture.srv rename to mesh_msgs/srv/GetTexture.srv diff --git a/mesh_msgs/service/GetUUIDs.srv b/mesh_msgs/srv/GetUUIDs.srv similarity index 100% rename from mesh_msgs/service/GetUUIDs.srv rename to mesh_msgs/srv/GetUUIDs.srv diff --git a/mesh_msgs/service/GetVertexColors.srv b/mesh_msgs/srv/GetVertexColors.srv similarity index 100% rename from mesh_msgs/service/GetVertexColors.srv rename to mesh_msgs/srv/GetVertexColors.srv diff --git a/mesh_msgs/service/GetVertexCostLayers.srv b/mesh_msgs/srv/GetVertexCostLayers.srv similarity index 100% rename from mesh_msgs/service/GetVertexCostLayers.srv rename to mesh_msgs/srv/GetVertexCostLayers.srv diff --git a/mesh_msgs/service/GetVertexCosts.srv b/mesh_msgs/srv/GetVertexCosts.srv similarity index 100% rename from mesh_msgs/service/GetVertexCosts.srv rename to mesh_msgs/srv/GetVertexCosts.srv From 6bfdd2d79df3382569e8f0997a07d6a6ccb0c28e Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 7 Nov 2023 03:02:37 +0100 Subject: [PATCH 08/40] c++17 for mesh_msgs and hdf5_map_io --- hdf5_map_io/CMakeLists.txt | 19 ++++++++++--------- mesh_msgs/CMakeLists.txt | 1 - 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/hdf5_map_io/CMakeLists.txt b/hdf5_map_io/CMakeLists.txt index 5e02edc..645b1fa 100644 --- a/hdf5_map_io/CMakeLists.txt +++ b/hdf5_map_io/CMakeLists.txt @@ -1,7 +1,14 @@ cmake_minimum_required(VERSION 3.8) project(hdf5_map_io) -set(CMAKE_CXX_STANDARD 14) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() # DEFAULT RELEASE if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) @@ -15,12 +22,6 @@ find_package(LVR2 REQUIRED) find_package(MPI) find_package(PkgConfig REQUIRED) -# catkin_package( -# INCLUDE_DIRS include -# LIBRARIES hdf5_map_io -# DEPENDS LVR2 MPI -# ) - # HighFive set(HIGHFIVE_EXAMPLES FALSE) set(HIGHFIVE_UNIT_TESTS FALSE) @@ -47,8 +48,8 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} +install(DIRECTORY include/ + DESTINATION include FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE ) diff --git a/mesh_msgs/CMakeLists.txt b/mesh_msgs/CMakeLists.txt index f5eda9b..3848de6 100644 --- a/mesh_msgs/CMakeLists.txt +++ b/mesh_msgs/CMakeLists.txt @@ -1,7 +1,6 @@ cmake_minimum_required(VERSION 3.8) project(mesh_msgs) - # Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) From 5ac5f0884ce427392fda82cad22231d2d5006171 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 7 Nov 2023 03:03:02 +0100 Subject: [PATCH 09/40] ported label_manager to ros2 humble --- label_manager/CMakeLists.txt | 100 ++++++---- label_manager/COLCON_IGNORE | 0 label_manager/include/label_manager/manager.h | 69 ++++--- label_manager/package.xml | 52 +++--- label_manager/src/manager.cpp | 173 +++++++++--------- label_manager/src/manager_node.cpp | 12 +- label_manager/srv/GetLabeledClusterGroup.srv | 2 +- 7 files changed, 228 insertions(+), 180 deletions(-) delete mode 100644 label_manager/COLCON_IGNORE diff --git a/label_manager/CMakeLists.txt b/label_manager/CMakeLists.txt index bf0409c..a78a4ed 100644 --- a/label_manager/CMakeLists.txt +++ b/label_manager/CMakeLists.txt @@ -1,52 +1,84 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(label_manager) -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs - genmsg - mesh_msgs - message_generation - roscpp - sensor_msgs - std_msgs - tf -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -add_service_files(FILES - DeleteLabel.srv - GetLabelGroups.srv - GetLabeledClusterGroup.srv -) +find_package(ament_cmake REQUIRED) -generate_messages(DEPENDENCIES - mesh_msgs - std_msgs + +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(actionlib_msgs REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(Boost COMPONENTS + system + filesystem ) -catkin_package( - CATKIN_DEPENDS - actionlib actionlib_msgs genmsg mesh_msgs message_generation message_runtime roscpp sensor_msgs std_msgs tf + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/DeleteLabel.srv" + "srv/GetLabelGroups.srv" + "srv/GetLabeledClusterGroup.srv" + DEPENDENCIES + mesh_msgs + std_msgs + ADD_LINTER_TESTS ) + include_directories( - include ${catkin_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + include ) - -add_executable(${PROJECT_NAME} +add_executable(${PROJECT_NAME}_node src/manager.cpp src/manager_node.cpp) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + +add_dependencies(${PROJECT_NAME}_node + ${PROJECT_NAME} +) + +target_link_libraries(${PROJECT_NAME}_node + Boost::system + Boost::filesystem +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + sensor_msgs + std_msgs ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) +rosidl_get_typesupport_target(cpp_typesupport_target + ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_node + "${cpp_typesupport_target}") + + +ament_export_dependencies(rosidl_default_runtime) +ament_package() + diff --git a/label_manager/COLCON_IGNORE b/label_manager/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/label_manager/include/label_manager/manager.h b/label_manager/include/label_manager/manager.h index 263f711..0e9c25d 100644 --- a/label_manager/include/label_manager/manager.h +++ b/label_manager/include/label_manager/manager.h @@ -2,46 +2,67 @@ #define LABEL_MANAGER_H_ #include +#include +#include +#include +#include + + + +#include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" +#include "mesh_msgs/srv/get_labeled_clusters.hpp" +#include "label_manager/srv/get_label_groups.hpp" +#include "label_manager/srv/get_labeled_cluster_group.hpp" +#include "label_manager/srv/delete_label.hpp" + +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; -#include -#include -#include -#include -#include -#include namespace label_manager { -class LabelManager +class LabelManager : public rclcpp::Node { public: - LabelManager(ros::NodeHandle& nodeHandle); + LabelManager(std::string handle_str = ""); private: - ros::NodeHandle nh; - ros::Subscriber clusterLabelSub; - ros::Publisher newClusterLabelPub; - ros::ServiceServer srv_get_labeled_clusters; - ros::ServiceServer srv_get_label_groups; - ros::ServiceServer srv_get_labeled_cluster_group; - ros::ServiceServer srv_delete_label; + // Subscriber + rclcpp::Subscription::SharedPtr + clusterLabelSub; + + // Publisher + rclcpp::Publisher::SharedPtr + newClusterLabelPub; + // Service (Servers) + rclcpp::Service::SharedPtr + srv_get_labeled_clusters; + rclcpp::Service::SharedPtr + srv_get_label_groups; + rclcpp::Service::SharedPtr + srv_get_labeled_cluster_group; + rclcpp::Service::SharedPtr + srv_delete_label; + std::string folderPath; - void clusterLabelCallback(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg); + void clusterLabelCallback(const mesh_msgs::msg::MeshFaceClusterStamped& msg); + bool service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request& req, - mesh_msgs::GetLabeledClusters::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getLabelGroups( - label_manager::GetLabelGroups::Request& req, - label_manager::GetLabelGroups::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getLabeledClusterGroup( - label_manager::GetLabeledClusterGroup::Request& req, - label_manager::GetLabeledClusterGroup::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool service_deleteLabel( - label_manager::DeleteLabel::Request& req, - label_manager::DeleteLabel::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool writeIndicesToFile(const std::string& fileName, const std::vector& indices, const bool append); std::vector readIndicesFromFile(const std::string& fileName); diff --git a/label_manager/package.xml b/label_manager/package.xml index 72d5ca2..5c6420c 100644 --- a/label_manager/package.xml +++ b/label_manager/package.xml @@ -1,9 +1,9 @@ - + + + label_manager - - Serving and persisting label information - 1.1.0 + Serving and persisting label information Sebastian Pütz http://wiki.ros.org/ros_mesh_tools/label_manager @@ -13,27 +13,27 @@ BSD-3 - actionlib_msgs - actionlib - genmsg - mesh_msgs - message_generation - roscpp - sensor_msgs - std_msgs - tf - - actionlib_msgs - actionlib - genmsg - mesh_msgs - message_generation - message_runtime - roscpp - sensor_msgs - std_msgs - tf - - catkin + ament_cmake + rosidl_default_generators + + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + sensor_msgs + std_msgs + + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/label_manager/src/manager.cpp b/label_manager/src/manager.cpp index e2310b8..d316c24 100644 --- a/label_manager/src/manager.cpp +++ b/label_manager/src/manager.cpp @@ -4,76 +4,73 @@ #include #include #include -#include "mesh_msgs/MeshFaceCluster.h" +#include "mesh_msgs/msg/mesh_face_cluster.h" using namespace boost::filesystem; +using std::placeholders::_1; +using std::placeholders::_2; + namespace label_manager { - LabelManager::LabelManager(ros::NodeHandle& nodeHandle) : - nh(nodeHandle) { - - if (!nh.getParam("folder_path", folderPath)) + LabelManager::LabelManager(std::string handle_str) + :rclcpp::Node(handle_str) + { + // TODO: params + // if(!nh.getParam("folder_path", folderPath)) + if(true) { folderPath = "/tmp/label_manager/"; } path p(folderPath); - if (!is_directory(p) && !exists(p)) + if(!is_directory(p) && !exists(p)) { create_directory(p); } - clusterLabelSub = nh.subscribe("cluster_label", 10, &LabelManager::clusterLabelCallback, this); - newClusterLabelPub = nh.advertise("new_cluster_label", 1); - srv_get_labeled_clusters = nh.advertiseService( - "get_labeled_clusters", - &LabelManager::service_getLabeledClusters, - this - ); - srv_get_label_groups = nh.advertiseService( - "get_label_groups", - &LabelManager::service_getLabelGroups, - this - ); - srv_get_labeled_cluster_group = nh.advertiseService( - "get_labeled_cluster_group", - &LabelManager::service_getLabeledClusterGroup, - this - ); - srv_delete_label = nh.advertiseService( - "delete_label", - &LabelManager::service_deleteLabel, - this - ); - - ROS_INFO("Started LabelManager"); - - ros::spin(); + clusterLabelSub = this->create_subscription( + "cluster_label", 10, std::bind(&LabelManager::clusterLabelCallback, this, _1)); + + newClusterLabelPub = this->create_publisher("new_cluster_label", 10); + + srv_get_labeled_clusters = this->create_service( + "get_labeled_clusters", std::bind(&LabelManager::service_getLabeledClusters, this, _1, _2)); + + srv_get_label_groups = this->create_service( + "get_label_groups", std::bind(&LabelManager::service_getLabelGroups, this, _1, _2)); + + srv_get_labeled_cluster_group = this->create_service( + "get_labeled_cluster_group", std::bind(&LabelManager::service_getLabeledClusterGroup, this, _1, _2)); + + srv_delete_label = this->create_service( + "delete_label", std::bind(&LabelManager::service_deleteLabel, this, _1, _2)); + + RCLCPP_INFO(this->get_logger(), "Started LabelManager"); } - void LabelManager::clusterLabelCallback(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg) + void LabelManager::clusterLabelCallback(const mesh_msgs::msg::MeshFaceClusterStamped& msg) { - ROS_INFO_STREAM("Got msg for mesh: " << msg->uuid << " with label: " << msg->cluster.label); + RCLCPP_INFO_STREAM(this->get_logger(), "Got msg for mesh: " << msg.uuid << " with label: " << msg.cluster.label); std::vector indices; - std::string fileName = getFileName(msg->uuid, msg->cluster.label); + std::string fileName = getFileName(msg.uuid, msg.cluster.label); // if appending (not override), first figure what new indices we have to add - if (!msg->override) + if (!msg.override) { std::vector readIndices = readIndicesFromFile(fileName); // if read indices is empty no file was found or could not be read if (readIndices.empty()) { - indices = msg->cluster.face_indices; + indices = msg.cluster.face_indices; } else { - for (size_t i = 0; i < msg->cluster.face_indices.size(); i++) + for (size_t i = 0; i < msg.cluster.face_indices.size(); i++) { - uint idx = msg->cluster.face_indices[i]; + uint idx = msg.cluster.face_indices[i]; // if msg index is not already in file, add it to indices vector if (std::find(readIndices.begin(), readIndices.end(), idx) == readIndices.end()) @@ -85,35 +82,35 @@ namespace label_manager } else { - indices = msg->cluster.face_indices; + indices = msg.cluster.face_indices; } // publish every new labeled cluster - newClusterLabelPub.publish(msg->cluster); + newClusterLabelPub->publish(msg.cluster); // make sure mesh folder exists before writing - path p(folderPath + "/" + msg->uuid); + path p(folderPath + "/" + msg.uuid); if (!is_directory(p) || !exists(p)) { create_directory(p); } - writeIndicesToFile(fileName, indices, !msg->override); + writeIndicesToFile(fileName, indices, !msg.override); } bool LabelManager::service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request& req, - mesh_msgs::GetLabeledClusters::Response& res - ) + const std::shared_ptr req, + std::shared_ptr res) { - ROS_DEBUG_STREAM("Service call with uuid: " << req.uuid); - path p (folderPath + "/" + req.uuid); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Service call with uuid: " << req->uuid); + + path p (folderPath + "/" + req->uuid); directory_iterator end_itr; if (!is_directory(p) || !exists(p)) { - ROS_DEBUG_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); + RCLCPP_DEBUG_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); return false; } @@ -127,11 +124,11 @@ namespace label_manager // remove extension from label boost::replace_all(label, itr->path().filename().extension().string(), ""); - mesh_msgs::MeshFaceCluster c; + mesh_msgs::msg::MeshFaceCluster c; c.face_indices = readIndicesFromFile(itr->path().string()); c.label = label; - res.clusters.push_back(c); + res->clusters.push_back(c); } } @@ -139,15 +136,15 @@ namespace label_manager } bool LabelManager::service_getLabelGroups( - label_manager::GetLabelGroups::Request& req, - label_manager::GetLabelGroups::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { - path p (folderPath + "/" + req.uuid); + path p (folderPath + "/" + req->uuid); directory_iterator end_itr; if (!is_directory(p) || !exists(p)) { - ROS_WARN_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); + RCLCPP_WARN_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); return false; } @@ -167,9 +164,9 @@ namespace label_manager label = label.substr(0, label.find_first_of("_", 0)); // only add label group to response if not already added - if (std::find(res.labels.begin(), res.labels.end(), label) == res.labels.end()) + if (std::find(res->labels.begin(), res->labels.end(), label) == res->labels.end()) { - res.labels.push_back(label); + res->labels.push_back(label); } } } @@ -178,35 +175,16 @@ namespace label_manager return true; } - bool LabelManager::service_deleteLabel( - label_manager::DeleteLabel::Request& req, - label_manager::DeleteLabel::Response& res) - { - path p(getFileName(req.uuid, req.label)); - - if (!is_regular_file(p) || !exists(p)) - { - ROS_WARN_STREAM("Could not delete label '" << req.label << "' of mesh '" << req.uuid << "'."); - - return false; - } - - res.cluster.face_indices = readIndicesFromFile(p.filename().string()); - res.cluster.label = req.label; - - return remove(p); - } - bool LabelManager::service_getLabeledClusterGroup( - label_manager::GetLabeledClusterGroup::Request& req, - label_manager::GetLabeledClusterGroup::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { - path p (folderPath + "/" + req.uuid); + path p (folderPath + "/" + req->uuid); directory_iterator end_itr; if (!is_directory(p) || !exists(p)) { - ROS_WARN_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); + RCLCPP_WARN_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); return false; } @@ -214,17 +192,17 @@ namespace label_manager for (directory_iterator itr(p); itr != end_itr; ++itr) { // if file is no dir - if (is_regular_file(itr->path()) && itr->path().filename().string().find(req.labelGroup) == 0) + if (is_regular_file(itr->path()) && itr->path().filename().string().find(req->label_group) == 0) { std::string label = itr->path().filename().string(); // remove extension from label boost::replace_all(label, itr->path().filename().extension().string(), ""); - mesh_msgs::MeshFaceCluster c; + mesh_msgs::msg::MeshFaceCluster c; c.face_indices = readIndicesFromFile(itr->path().string()); c.label = label; - res.clusters.push_back(c); + res->clusters.push_back(c); } } @@ -232,6 +210,25 @@ namespace label_manager return true; } + bool LabelManager::service_deleteLabel( + const std::shared_ptr req, + std::shared_ptr res) + { + path p(getFileName(req->uuid, req->label)); + + if (!is_regular_file(p) || !exists(p)) + { + RCLCPP_WARN_STREAM(this->get_logger(), "Could not delete label '" << req->label << "' of mesh '" << req->uuid << "'."); + + return false; + } + + res->cluster.face_indices = readIndicesFromFile(p.filename().string()); + res->cluster.label = req->label; + + return remove(p); + } + bool LabelManager::writeIndicesToFile( const std::string& fileName, const std::vector& indices, @@ -240,7 +237,7 @@ namespace label_manager { if (indices.empty()) { - ROS_WARN_STREAM("Empty indices."); + RCLCPP_WARN_STREAM(this->get_logger(), "Empty indices."); return true; } @@ -248,7 +245,7 @@ namespace label_manager std::ios_base::openmode mode = append ? (std::ios::out|std::ios::app) : std::ios::out; std::ofstream ofs(fileName.c_str(), mode); - ROS_DEBUG_STREAM("Writing indices to file: " << fileName); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Writing indices to file: " << fileName); if (ofs.is_open()) { @@ -270,13 +267,13 @@ namespace label_manager } ofs.close(); - ROS_DEBUG_STREAM("Successfully written indices to file."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Successfully written indices to file."); return true; } else { - ROS_ERROR_STREAM("Could not open file: " << fileName); + RCLCPP_ERROR_STREAM(this->get_logger(), "Could not open file: " << fileName); } return false; @@ -290,7 +287,7 @@ namespace label_manager // if file dos not exists, return empty vector if (!ifs.good()) { - ROS_DEBUG_STREAM("File " << fileName << " does not exists. Nothing to read..."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "File " << fileName << " does not exists. Nothing to read..."); return faceIndices; } diff --git a/label_manager/src/manager_node.cpp b/label_manager/src/manager_node.cpp index d18cd97..ac5fff9 100644 --- a/label_manager/src/manager_node.cpp +++ b/label_manager/src/manager_node.cpp @@ -3,16 +3,14 @@ * */ -#include +#include "rclcpp/rclcpp.hpp" #include "label_manager/manager.h" int main(int argc, char **argv) { - ros::init(argc, argv, "label_manager"); - ros::NodeHandle nodeHandle("~"); - - label_manager::LabelManager lm(nodeHandle); - - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared( + "label_manager")); + rclcpp::shutdown(); return 0; } diff --git a/label_manager/srv/GetLabeledClusterGroup.srv b/label_manager/srv/GetLabeledClusterGroup.srv index 9591e20..67d4ef3 100644 --- a/label_manager/srv/GetLabeledClusterGroup.srv +++ b/label_manager/srv/GetLabeledClusterGroup.srv @@ -1,4 +1,4 @@ string uuid -string labelGroup +string label_group --- mesh_msgs/MeshFaceCluster[] clusters From 0187c1df8d59bbcacfe09e15c4f539a41f0b9a3d Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 7 Nov 2023 03:10:41 +0100 Subject: [PATCH 10/40] added params --- label_manager/src/manager.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/label_manager/src/manager.cpp b/label_manager/src/manager.cpp index d316c24..4409b3b 100644 --- a/label_manager/src/manager.cpp +++ b/label_manager/src/manager.cpp @@ -16,12 +16,10 @@ namespace label_manager LabelManager::LabelManager(std::string handle_str) :rclcpp::Node(handle_str) { - // TODO: params - // if(!nh.getParam("folder_path", folderPath)) - if(true) - { - folderPath = "/tmp/label_manager/"; - } + // TODO: check if this is correct + this->declare_parameter("folder_path", "/tmp/label_manager/"); + folderPath = this->get_parameter("folder_path").as_string(); + path p(folderPath); if(!is_directory(p) && !exists(p)) From af7f512e2a002919df8d9d0c5ee59d24a12bad96 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 8 Nov 2023 00:20:07 +0100 Subject: [PATCH 11/40] ported mesh_msgs_hdf5. fixed library install / export of hdf5_map_io --- hdf5_map_io/CMakeLists.txt | 35 ++- hdf5_map_io/package.xml | 1 + label_manager/include/label_manager/manager.h | 4 - label_manager/package.xml | 2 - label_manager/src/manager.cpp | 3 +- mesh_msgs_hdf5/CMakeLists.txt | 61 ++-- mesh_msgs_hdf5/COLCON_IGNORE | 0 .../include/mesh_msgs_hdf5/mesh_msgs_hdf5.h | 136 ++++----- mesh_msgs_hdf5/package.xml | 22 +- mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp | 279 ++++++++++-------- 10 files changed, 298 insertions(+), 245 deletions(-) delete mode 100644 mesh_msgs_hdf5/COLCON_IGNORE diff --git a/hdf5_map_io/CMakeLists.txt b/hdf5_map_io/CMakeLists.txt index 645b1fa..724a47c 100644 --- a/hdf5_map_io/CMakeLists.txt +++ b/hdf5_map_io/CMakeLists.txt @@ -18,40 +18,59 @@ if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) endif() find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) find_package(LVR2 REQUIRED) find_package(MPI) find_package(PkgConfig REQUIRED) + # HighFive set(HIGHFIVE_EXAMPLES FALSE) set(HIGHFIVE_UNIT_TESTS FALSE) +find_library(LVR2_LIBRARY NAMES lvr2) + + include_directories( include ${LVR2_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/hdf5_map_io.cpp ) -find_library(LVR2_LIBRARY NAMES lvr2) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME} PUBLIC ${LVR2_LIBRARY} ${MPI_CXX_LIBRARIES} + rclcpp::rclcpp ) +target_compile_definitions(${PROJECT_NAME} PRIVATE "HDF5_MAP_IO_BUILDING_DLL") + install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION bin/${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +ament_export_dependencies( + rclcpp ) ament_package() \ No newline at end of file diff --git a/hdf5_map_io/package.xml b/hdf5_map_io/package.xml index 535e5e6..525b994 100644 --- a/hdf5_map_io/package.xml +++ b/hdf5_map_io/package.xml @@ -10,6 +10,7 @@ Sebastian Pütz ament_cmake + rclcpp boost lvr2 diff --git a/label_manager/include/label_manager/manager.h b/label_manager/include/label_manager/manager.h index 0e9c25d..754cfc2 100644 --- a/label_manager/include/label_manager/manager.h +++ b/label_manager/include/label_manager/manager.h @@ -7,8 +7,6 @@ #include #include - - #include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" #include "mesh_msgs/srv/get_labeled_clusters.hpp" #include "label_manager/srv/get_label_groups.hpp" @@ -17,8 +15,6 @@ #include "rclcpp/rclcpp.hpp" -using namespace std::chrono_literals; - namespace label_manager { diff --git a/label_manager/package.xml b/label_manager/package.xml index 5c6420c..6e09693 100644 --- a/label_manager/package.xml +++ b/label_manager/package.xml @@ -34,6 +34,4 @@ ament_cmake - - diff --git a/label_manager/src/manager.cpp b/label_manager/src/manager.cpp index 4409b3b..99fdad9 100644 --- a/label_manager/src/manager.cpp +++ b/label_manager/src/manager.cpp @@ -30,7 +30,8 @@ namespace label_manager clusterLabelSub = this->create_subscription( "cluster_label", 10, std::bind(&LabelManager::clusterLabelCallback, this, _1)); - newClusterLabelPub = this->create_publisher("new_cluster_label", 10); + newClusterLabelPub = this->create_publisher( + "new_cluster_label", 10); srv_get_labeled_clusters = this->create_service( "get_labeled_clusters", std::bind(&LabelManager::service_getLabeledClusters, this, _1, _2)); diff --git a/mesh_msgs_hdf5/CMakeLists.txt b/mesh_msgs_hdf5/CMakeLists.txt index 60b34ff..8473dda 100644 --- a/mesh_msgs_hdf5/CMakeLists.txt +++ b/mesh_msgs_hdf5/CMakeLists.txt @@ -1,27 +1,32 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.8) project(mesh_msgs_hdf5) -set(PACKAGE_DEPENDENCIES - mesh_msgs - hdf5_map_io - label_manager -) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) -find_package(HDF5 REQUIRED COMPONENTS C CXX HL) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -### compile with c++14 -set(CMAKE_CXX_STANDARD 14) -catkin_package( - CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} - DEPENDS HDF5 -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(actionlib_msgs REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(hdf5_map_io REQUIRED) +find_package(label_manager REQUIRED) + +find_package(HDF5 REQUIRED COMPONENTS C CXX HL) + include_directories( include - ${catkin_INCLUDE_DIRS} ${HDF5_INCLUDE_DIRS} ) @@ -30,23 +35,31 @@ add_executable(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + hdf5_map_io + label_manager) + + install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include ) + +ament_package() diff --git a/mesh_msgs_hdf5/COLCON_IGNORE b/mesh_msgs_hdf5/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h index f6c608e..e14b173 100644 --- a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h +++ b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h @@ -1,120 +1,120 @@ #ifndef MESH_MSGS_HDF5_H_ #define MESH_MSGS_HDF5_H_ -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include +#include + +#include + +#include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" +#include "mesh_msgs/srv/get_geometry.hpp" +#include "mesh_msgs/srv/get_materials.hpp" +#include "mesh_msgs/srv/get_texture.hpp" +#include "mesh_msgs/srv/get_uui_ds.hpp" // :D +#include "mesh_msgs/srv/get_vertex_colors.hpp" +#include "mesh_msgs/srv/get_vertex_costs.hpp" +#include "mesh_msgs/srv/get_vertex_cost_layers.hpp" +#include "mesh_msgs/srv/get_labeled_clusters.hpp" +#include "label_manager/srv/get_label_groups.hpp" +#include "label_manager/srv/get_labeled_cluster_group.hpp" +#include "label_manager/srv/delete_label.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/fill_image.hpp" + +#include "rclcpp/rclcpp.hpp" namespace mesh_msgs_hdf5 { -class hdf5_to_msg +class hdf5_to_msg : public rclcpp::Node { public: - hdf5_to_msg(); + hdf5_to_msg(std::string handle_str = "mesh_msgs_hdf5"); protected: void loadAndPublishGeometry(); - bool getVertices(std::vector& vertices, mesh_msgs::MeshGeometryStamped& geometryMsg); - bool getFaces(std::vector& faceIds, mesh_msgs::MeshGeometryStamped& geometryMsg); - bool getVertexNormals(std::vector& vertexNormals, mesh_msgs::MeshGeometryStamped& geometryMsg); + bool getVertices(std::vector& vertices, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); + bool getFaces(std::vector& faceIds, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); + bool getVertexNormals(std::vector& vertexNormals, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); - bool getVertexColors(std::vector& vertexColors, mesh_msgs::MeshVertexColorsStamped& vertexColorsMsg); - bool getVertexCosts(std::vector& vertexCosts, std::string layer, mesh_msgs::MeshVertexCostsStamped& vertexCostsMsg); + bool getVertexColors(std::vector& vertexColors, mesh_msgs::msg::MeshVertexColorsStamped& vertexColorsMsg); + bool getVertexCosts(std::vector& vertexCosts, std::string layer, mesh_msgs::msg::MeshVertexCostsStamped& vertexCostsMsg); // Mesh services bool service_getUUIDs( - mesh_msgs::GetUUIDs::Request &req, - mesh_msgs::GetUUIDs::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometry( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometryVertices( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometryFaces( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometryVertexNormals( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getMaterials( - mesh_msgs::GetMaterials::Request &req, - mesh_msgs::GetMaterials::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getTexture( - mesh_msgs::GetTexture::Request &req, - mesh_msgs::GetTexture::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getVertexColors( - mesh_msgs::GetVertexColors::Request &req, - mesh_msgs::GetVertexColors::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getVertexCosts( - mesh_msgs::GetVertexCosts::Request &req, - mesh_msgs::GetVertexCosts::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getVertexCostLayers( - mesh_msgs::GetVertexCostLayers::Request &req, - mesh_msgs::GetVertexCostLayers::Response &res); + const std::shared_ptr req, + std::shared_ptr res); // Label manager services bool service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request &req, - mesh_msgs::GetLabeledClusters::Response &res); + const std::shared_ptr req, + std::shared_ptr res); - void callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped::ConstPtr &msg); + void callback_clusterLabel(const mesh_msgs::msg::MeshFaceClusterStamped& msg); private: // Mesh message service servers - ros::ServiceServer srv_get_uuids_; - ros::ServiceServer srv_get_geometry_; - ros::ServiceServer srv_get_geometry_vertices_; - ros::ServiceServer srv_get_geometry_faces_; - ros::ServiceServer srv_get_geometry_vertex_normals_; - ros::ServiceServer srv_get_materials_; - ros::ServiceServer srv_get_texture_; - ros::ServiceServer srv_get_vertex_colors_; - ros::ServiceServer srv_get_vertex_costs_; - ros::ServiceServer srv_get_vertex_cost_layers_; + + rclcpp::Service::SharedPtr srv_get_uuids_; + rclcpp::Service::SharedPtr srv_get_geometry_; + rclcpp::Service::SharedPtr srv_get_geometry_vertices_; + rclcpp::Service::SharedPtr srv_get_geometry_faces_; + rclcpp::Service::SharedPtr srv_get_geometry_vertex_normals_; + rclcpp::Service::SharedPtr srv_get_materials_; + rclcpp::Service::SharedPtr srv_get_texture_; + rclcpp::Service::SharedPtr srv_get_vertex_colors_; + rclcpp::Service::SharedPtr srv_get_vertex_costs_; + rclcpp::Service::SharedPtr srv_get_vertex_cost_layers_; // Mesh message publishers - ros::Publisher pub_geometry_; - ros::Publisher pub_vertex_colors_; - ros::Publisher pub_vertex_costs_; + rclcpp::Publisher::SharedPtr pub_geometry_; + rclcpp::Publisher::SharedPtr pub_vertex_colors_; + rclcpp::Publisher::SharedPtr pub_vertex_costs_; // Label manager services and subs/pubs - ros::ServiceServer srv_get_labeled_clusters_; - ros::Subscriber sub_cluster_label_; - - // ROS - ros::NodeHandle node_handle; + rclcpp::Service::SharedPtr srv_get_labeled_clusters_; + rclcpp::Subscription::SharedPtr sub_cluster_label_; // ROS parameter std::string inputFile; std::string mesh_uuid = "mesh"; - }; } // end namespace diff --git a/mesh_msgs_hdf5/package.xml b/mesh_msgs_hdf5/package.xml index d3caf7f..b69cd08 100644 --- a/mesh_msgs_hdf5/package.xml +++ b/mesh_msgs_hdf5/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs_hdf5 1.1.0 Read mesh_msgs from hdf5 @@ -8,14 +9,19 @@ BSD-3 Sebastian Pütz - mesh_msgs - hdf5_map_io - label_manager + ament_cmake - mesh_msgs - hdf5_map_io - label_manager + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + hdf5_map_io + label_manager - catkin + ament_lint_common + + ament_cmake + diff --git a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp index 98ace78..497b257 100644 --- a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp +++ b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp @@ -1,49 +1,56 @@ #include #include +#include + +using std::placeholders::_1; +using std::placeholders::_2; namespace mesh_msgs_hdf5 { -hdf5_to_msg::hdf5_to_msg() +hdf5_to_msg::hdf5_to_msg(std::string handle_str) +:rclcpp::Node(handle_str) { - ros::NodeHandle nh("~"); - - if (!nh.getParam("inputFile", inputFile)) - { - inputFile = "/tmp/map.h5"; - } + // TODO: check if this is correct + this->declare_parameter("inputFile", "/tmp/map.h5"); + inputFile = this->get_parameter("inputFile").as_string(); + + RCLCPP_INFO_STREAM(this->get_logger(), "Using input file: " << inputFile); + + srv_get_uuids_ = this->create_service( + "get_uuids", std::bind(&hdf5_to_msg::service_getUUIDs, this, _1, _2)); + srv_get_geometry_ = this->create_service( + "get_geometry", std::bind(&hdf5_to_msg::service_getGeometry, this, _1, _2)); + srv_get_geometry_vertices_ = this->create_service( + "get_geometry_vertices", std::bind(&hdf5_to_msg::service_getGeometryVertices, this, _1, _2)); + srv_get_geometry_faces_ = this->create_service( + "get_geometry_faces", std::bind(&hdf5_to_msg::service_getGeometryFaces, this, _1, _2)); + srv_get_geometry_vertex_normals_ = this->create_service( + "get_geometry_vertexnormals", std::bind(&hdf5_to_msg::service_getGeometryVertexNormals, this, _1, _2)); + srv_get_materials_ = this->create_service( + "get_materials", std::bind(&hdf5_to_msg::service_getMaterials, this, _1, _2)); + srv_get_texture_ = this->create_service( + "get_texture", std::bind(&hdf5_to_msg::service_getTexture, this, _1, _2)); + srv_get_vertex_colors_ = this->create_service( + "get_vertex_colors", std::bind(&hdf5_to_msg::service_getVertexColors, this, _1, _2)); + srv_get_vertex_costs_ = this->create_service( + "get_vertex_costs", std::bind(&hdf5_to_msg::service_getVertexCosts, this, _1, _2)); + srv_get_vertex_cost_layers_ = this->create_service( + "get_vertex_cost_layers", std::bind(&hdf5_to_msg::service_getVertexCostLayers, this, _1, _2)); + + pub_geometry_ = this->create_publisher( + "mesh/geometry", 1); + pub_vertex_colors_ = this->create_publisher( + "mesh/vertex_colors", 1); + pub_vertex_costs_ = this->create_publisher( + "mesh/vertex_costs", 1); + + + srv_get_labeled_clusters_ = this->create_service( + "get_labeled_clusters", std::bind(&hdf5_to_msg::service_getLabeledClusters, this, _1, _2)); + + sub_cluster_label_ = this->create_subscription( + "cluster_label", 10, std::bind(&hdf5_to_msg::callback_clusterLabel, this, _1)); - ROS_INFO_STREAM("Using input file: " << inputFile); - - srv_get_geometry_ = node_handle.advertiseService( - "get_geometry", &hdf5_to_msg::service_getGeometry, this); - srv_get_geometry_vertices_ = node_handle.advertiseService( - "get_geometry_vertices", &hdf5_to_msg::service_getGeometryVertices, this); - srv_get_geometry_faces_ = node_handle.advertiseService( - "get_geometry_faces", &hdf5_to_msg::service_getGeometryFaces, this); - srv_get_geometry_vertex_normals_ = node_handle.advertiseService( - "get_geometry_vertexnormals", &hdf5_to_msg::service_getGeometryVertexNormals, this); - srv_get_materials_ = node_handle.advertiseService( - "get_materials", &hdf5_to_msg::service_getMaterials, this); - srv_get_texture_ = node_handle.advertiseService( - "get_texture", &hdf5_to_msg::service_getTexture, this); - srv_get_uuids_ = node_handle.advertiseService( - "get_uuids", &hdf5_to_msg::service_getUUIDs, this); - srv_get_vertex_colors_ = node_handle.advertiseService( - "get_vertex_colors", &hdf5_to_msg::service_getVertexColors, this); - srv_get_vertex_costs_ = node_handle.advertiseService( - "get_vertex_costs", &hdf5_to_msg::service_getVertexCosts, this); - srv_get_vertex_cost_layers_ = node_handle.advertiseService( - "get_vertex_cost_layers", &hdf5_to_msg::service_getVertexCostLayers, this); - - pub_geometry_ = node_handle.advertise("mesh/geometry", 1, true); - pub_vertex_colors_ = node_handle.advertise("mesh/vertex_colors", 1, true); - pub_vertex_costs_ = node_handle.advertise("mesh/vertex_costs", 1); - - - srv_get_labeled_clusters_ = node_handle.advertiseService( - "get_labeled_clusters", &hdf5_to_msg::service_getLabeledClusters, this); - - sub_cluster_label_ = node_handle.subscribe("cluster_label", 10, &hdf5_to_msg::callback_clusterLabel, this); loadAndPublishGeometry(); } @@ -53,7 +60,7 @@ void hdf5_to_msg::loadAndPublishGeometry() hdf5_map_io::HDF5MapIO io(inputFile); // geometry - mesh_msgs::MeshGeometryStamped geometryMsg; + mesh_msgs::msg::MeshGeometryStamped geometryMsg; auto vertices = io.getVertices(); auto faceIds = io.getFaceIds(); @@ -63,18 +70,18 @@ void hdf5_to_msg::loadAndPublishGeometry() getFaces(faceIds, geometryMsg); getVertexNormals(vertexNormals, geometryMsg); - pub_geometry_.publish(geometryMsg); + pub_geometry_->publish(geometryMsg); // vertex colors - mesh_msgs::MeshVertexColorsStamped vertexColorsMsg; + mesh_msgs::msg::MeshVertexColorsStamped vertexColorsMsg; auto vertexColors = io.getVertexColors(); getVertexColors(vertexColors, vertexColorsMsg); - pub_vertex_colors_.publish(vertexColorsMsg); + pub_vertex_colors_->publish(vertexColorsMsg); // vertex costs - mesh_msgs::MeshVertexCostsStamped vertexCostsMsg; + mesh_msgs::msg::MeshVertexCostsStamped vertexCostsMsg; for (std::string costlayer : io.getCostLayers()) { try @@ -82,19 +89,21 @@ void hdf5_to_msg::loadAndPublishGeometry() auto costs = io.getVertexCosts(costlayer); getVertexCosts(costs, costlayer, vertexCostsMsg); - pub_vertex_costs_.publish(vertexCostsMsg); + pub_vertex_costs_->publish(vertexCostsMsg); } catch (const hf::DataSpaceException& e) { - ROS_WARN_STREAM("Could not load costlayer " << costlayer); + RCLCPP_WARN_STREAM(this->get_logger(), "Could not load costlayer " << costlayer); } } } -bool hdf5_to_msg::getVertices(std::vector& vertices, mesh_msgs::MeshGeometryStamped& geometryMsg) +bool hdf5_to_msg::getVertices( + std::vector& vertices, + mesh_msgs::msg::MeshGeometryStamped& geometryMsg) { unsigned int nVertices = vertices.size() / 3; - ROS_INFO_STREAM("Found " << nVertices << " vertices"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertices << " vertices"); geometryMsg.mesh_geometry.vertices.resize(nVertices); for (unsigned int i = 0; i < nVertices; i++) { @@ -106,15 +115,17 @@ bool hdf5_to_msg::getVertices(std::vector& vertices, mesh_msgs::MeshGeome // Header geometryMsg.uuid = mesh_uuid; geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = ros::Time::now(); + geometryMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getFaces(std::vector& faceIds, mesh_msgs::MeshGeometryStamped& geometryMsg) +bool hdf5_to_msg::getFaces( + std::vector& faceIds, + mesh_msgs::msg::MeshGeometryStamped& geometryMsg) { unsigned int nFaces = faceIds.size() / 3; - ROS_INFO_STREAM("Found " << nFaces << " faces"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nFaces << " faces"); geometryMsg.mesh_geometry.faces.resize(nFaces); for (unsigned int i = 0; i < nFaces; i++) { @@ -126,15 +137,17 @@ bool hdf5_to_msg::getFaces(std::vector& faceIds, mesh_msgs::MeshGeomet // Header geometryMsg.uuid = mesh_uuid; geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = ros::Time::now(); + geometryMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getVertexNormals(std::vector& vertexNormals, mesh_msgs::MeshGeometryStamped& geometryMsg) +bool hdf5_to_msg::getVertexNormals( + std::vector& vertexNormals, + mesh_msgs::msg::MeshGeometryStamped& geometryMsg) { unsigned int nVertexNormals = vertexNormals.size() / 3; - ROS_INFO_STREAM("Found " << nVertexNormals << " vertex normals"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertexNormals << " vertex normals"); geometryMsg.mesh_geometry.vertex_normals.resize(nVertexNormals); for (unsigned int i = 0; i < nVertexNormals; i++) { @@ -146,15 +159,17 @@ bool hdf5_to_msg::getVertexNormals(std::vector& vertexNormals, mesh_msgs: // Header geometryMsg.uuid = mesh_uuid; geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = ros::Time::now(); + geometryMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getVertexColors(std::vector& vertexColors, mesh_msgs::MeshVertexColorsStamped& vertexColorsMsg) +bool hdf5_to_msg::getVertexColors( + std::vector& vertexColors, + mesh_msgs::msg::MeshVertexColorsStamped& vertexColorsMsg) { unsigned int nVertices = vertexColors.size() / 3; - ROS_INFO_STREAM("Found " << nVertices << " vertices for vertex colors"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertices << " vertices for vertex colors"); vertexColorsMsg.mesh_vertex_colors.vertex_colors.resize(nVertices); for (unsigned int i = 0; i < nVertices; i++) { @@ -171,12 +186,15 @@ bool hdf5_to_msg::getVertexColors(std::vector& vertexColors, mesh_msgs: // Header vertexColorsMsg.uuid = mesh_uuid; vertexColorsMsg.header.frame_id = "map"; - vertexColorsMsg.header.stamp = ros::Time::now(); + vertexColorsMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getVertexCosts(std::vector& costs, std::string layer, mesh_msgs::MeshVertexCostsStamped& vertexCostsMsg) +bool hdf5_to_msg::getVertexCosts( + std::vector& costs, + std::string layer, + mesh_msgs::msg::MeshVertexCostsStamped& vertexCostsMsg) { vertexCostsMsg.mesh_vertex_costs.costs.resize(costs.size()); for (uint32_t i = 0; i < costs.size(); i++) @@ -187,76 +205,76 @@ bool hdf5_to_msg::getVertexCosts(std::vector& costs, std::string layer, m vertexCostsMsg.uuid = mesh_uuid; vertexCostsMsg.type = layer; vertexCostsMsg.header.frame_id = "map"; - vertexCostsMsg.header.stamp = ros::Time::now(); + vertexCostsMsg.header.stamp = this->get_clock()->now(); return true; } bool hdf5_to_msg::service_getUUIDs( - mesh_msgs::GetUUIDs::Request& req, - mesh_msgs::GetUUIDs::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { - res.uuids.push_back(mesh_uuid); + res->uuids.push_back(mesh_uuid); return true; } bool hdf5_to_msg::service_getGeometry( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertices auto vertices = io.getVertices(); - getVertices(vertices, res.mesh_geometry_stamped); + getVertices(vertices, res->mesh_geometry_stamped); // Faces auto faceIds = io.getFaceIds(); - getFaces(faceIds, res.mesh_geometry_stamped); + getFaces(faceIds, res->mesh_geometry_stamped); // Vertex normals auto vertexNormals = io.getVertexNormals(); - getVertexNormals(vertexNormals, res.mesh_geometry_stamped); + getVertexNormals(vertexNormals, res->mesh_geometry_stamped); return true; } bool hdf5_to_msg::service_getGeometryVertices( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertices auto vertices = io.getVertices(); - return getVertices(vertices, res.mesh_geometry_stamped); + return getVertices(vertices, res->mesh_geometry_stamped); } bool hdf5_to_msg::service_getGeometryFaces( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Faces auto faceIds = io.getFaceIds(); - return getFaces(faceIds, res.mesh_geometry_stamped); + return getFaces(faceIds, res->mesh_geometry_stamped); } bool hdf5_to_msg::service_getGeometryVertexNormals( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertex normals auto vertexNormals = io.getVertexNormals(); - return getVertexNormals(vertexNormals, res.mesh_geometry_stamped); + return getVertexNormals(vertexNormals, res->mesh_geometry_stamped); } bool hdf5_to_msg::service_getMaterials( - mesh_msgs::GetMaterials::Request& req, - mesh_msgs::GetMaterials::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); @@ -266,29 +284,29 @@ bool hdf5_to_msg::service_getMaterials( auto materialFaceIndices = io.getMaterialFaceIndices(); // for each face: material index unsigned int nMaterials = materials.size(); unsigned int nFaces = materialFaceIndices.size(); - ROS_INFO_STREAM("Found " << nMaterials << " materials and " << nFaces << " faces"); - res.mesh_materials_stamped.mesh_materials.materials.resize(nMaterials); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nMaterials << " materials and " << nFaces << " faces"); + res->mesh_materials_stamped.mesh_materials.materials.resize(nMaterials); for (uint32_t i = 0; i < nMaterials; i++) { int texture_index = materials[i].textureIndex; // has texture - res.mesh_materials_stamped + res->mesh_materials_stamped .mesh_materials .materials[i] .has_texture = texture_index >= 0; // texture index - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .texture_index = static_cast(texture_index); // color - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.r = materials[i].r / 255.0f; - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.g = materials[i].g / 255.0f; - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.b = materials[i].b / 255.0f; - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.a = 1; } @@ -309,53 +327,53 @@ bool hdf5_to_msg::service_getMaterials( materialToFaces[materialIndex].push_back(i); } // For each material, map contains a list of faces - res.mesh_materials_stamped.mesh_materials.clusters.resize(nMaterials); + res->mesh_materials_stamped.mesh_materials.clusters.resize(nMaterials); for (uint32_t i = 0; i < nMaterials; i++) { for (uint32_t j = 0; j < materialToFaces[i].size(); j++) { - res.mesh_materials_stamped.mesh_materials.clusters[i].face_indices.push_back(materialToFaces[i][j]); + res->mesh_materials_stamped.mesh_materials.clusters[i].face_indices.push_back(materialToFaces[i][j]); } } - res.mesh_materials_stamped.mesh_materials.cluster_materials.resize(nMaterials); + res->mesh_materials_stamped.mesh_materials.cluster_materials.resize(nMaterials); for (uint32_t i = 0; i < nMaterials; i++) { - res.mesh_materials_stamped.mesh_materials.cluster_materials[i] = i; + res->mesh_materials_stamped.mesh_materials.cluster_materials[i] = i; } // Vertex Tex Coords auto vertexTexCoords = io.getVertexTextureCoords(); unsigned int nVertices = vertexTexCoords.size() / 3; - res.mesh_materials_stamped.mesh_materials.vertex_tex_coords.resize(nVertices); + res->mesh_materials_stamped.mesh_materials.vertex_tex_coords.resize(nVertices); for (uint32_t i = 0; i < nVertices; i++) { // coords: u/v/w // w is always 0 - res.mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].u = vertexTexCoords[3 * i]; - res.mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].v = vertexTexCoords[3 * i + 1]; + res->mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].u = vertexTexCoords[3 * i]; + res->mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].v = vertexTexCoords[3 * i + 1]; } // Header - res.mesh_materials_stamped.uuid = mesh_uuid; - res.mesh_materials_stamped.header.frame_id = "map"; - res.mesh_materials_stamped.header.stamp = ros::Time::now(); + res->mesh_materials_stamped.uuid = mesh_uuid; + res->mesh_materials_stamped.header.frame_id = "map"; + res->mesh_materials_stamped.header.stamp = this->get_clock()->now(); return true; } bool hdf5_to_msg::service_getTexture( - mesh_msgs::GetTexture::Request& req, - mesh_msgs::GetTexture::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); for (auto texture : io.getTextures()) { - if (std::stoi(texture.name) == req.texture_index) + if (std::stoi(texture.name) == req->texture_index) { - res.texture.texture_index = req.texture_index; - res.texture.uuid = mesh_uuid; - sensor_msgs::Image image; + res->texture.texture_index = req->texture_index; + res->texture.uuid = mesh_uuid; + sensor_msgs::msg::Image image; sensor_msgs::fillImage( // TODO: only RGB, breaks when using other color channels image, "rgb8", @@ -365,7 +383,7 @@ bool hdf5_to_msg::service_getTexture( texture.data.data() ); - res.texture.image = image; + res->texture.image = image; return true; } @@ -375,39 +393,39 @@ bool hdf5_to_msg::service_getTexture( } bool hdf5_to_msg::service_getVertexColors( - mesh_msgs::GetVertexColors::Request& req, - mesh_msgs::GetVertexColors::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertex colors auto vertexColors = io.getVertexColors(); - return getVertexColors(vertexColors, res.mesh_vertex_colors_stamped); + return getVertexColors(vertexColors, res->mesh_vertex_colors_stamped); } bool hdf5_to_msg::service_getVertexCosts( - mesh_msgs::GetVertexCosts::Request& req, - mesh_msgs::GetVertexCosts::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); - auto costs = io.getVertexCosts(req.layer); - return getVertexCosts(costs, req.layer, res.mesh_vertex_costs_stamped); + auto costs = io.getVertexCosts(req->layer); + return getVertexCosts(costs, req->layer, res->mesh_vertex_costs_stamped); } bool hdf5_to_msg::service_getVertexCostLayers( - mesh_msgs::GetVertexCostLayers::Request &req, - mesh_msgs::GetVertexCostLayers::Response &res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); - res.layers = io.getCostLayers(); + res->layers = io.getCostLayers(); return true; } bool hdf5_to_msg::service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request& req, - mesh_msgs::GetLabeledClusters::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); @@ -421,7 +439,7 @@ bool hdf5_to_msg::service_getLabeledClusters( { // copy label auto faceIds = io.getFaceIdsOfLabel(groups[i], labelsInGroup[j]); - mesh_msgs::MeshFaceCluster cluster; + mesh_msgs::msg::MeshFaceCluster cluster; std::stringstream ss; ss << groups[i] << "_" << labelsInGroup[j]; cluster.label = ss.str(); @@ -430,33 +448,34 @@ bool hdf5_to_msg::service_getLabeledClusters( { cluster.face_indices[k] = faceIds[k]; } - res.clusters.push_back(cluster); + res->clusters.push_back(cluster); } } return true; } -void hdf5_to_msg::callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg) +void hdf5_to_msg::callback_clusterLabel( + const mesh_msgs::msg::MeshFaceClusterStamped& msg) { - if (msg->uuid.compare(mesh_uuid) != 0) + if (msg.uuid.compare(mesh_uuid) != 0) { - ROS_ERROR("Invalid mesh UUID"); + RCLCPP_ERROR(this->get_logger(), "Invalid mesh UUID"); return; } hdf5_map_io::HDF5MapIO io(inputFile); // TODO: implement optional override - ROS_WARN("Override is enabled by default"); + RCLCPP_WARN(this->get_logger(), "Override is enabled by default"); // split label id into group and name std::vector split_results; - boost::split(split_results, msg->cluster.label, [](char c){ return c == '_'; }); + boost::split(split_results, msg.cluster.label, [](char c){ return c == '_'; }); if (split_results.size() != 2) { - ROS_ERROR("Received illegal cluster name"); + RCLCPP_ERROR(this->get_logger(), "Received illegal cluster name"); return; } @@ -464,9 +483,9 @@ void hdf5_to_msg::callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped: std::string label_group = split_results[0]; std::string label_name = split_results[1]; std::vector indices; - for (size_t i = 0; i < msg->cluster.face_indices.size(); i++) + for (size_t i = 0; i < msg.cluster.face_indices.size(); i++) { - indices.push_back(msg->cluster.face_indices[i]); + indices.push_back(msg.cluster.face_indices[i]); } // write to hdf5 @@ -477,9 +496,9 @@ void hdf5_to_msg::callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped: int main(int argc, char **args) { - ros::init(argc, args, "mesh_msgs_hdf5"); - mesh_msgs_hdf5::hdf5_to_msg hdf5_to_msg; - ros::MultiThreadedSpinner spinner(4); - spinner.spin(); + // ros::init(argc, args, "mesh_msgs_hdf5"); + // mesh_msgs_hdf5::hdf5_to_msg hdf5_to_msg; + // ros::MultiThreadedSpinner spinner(4); + // spinner.spin(); return 0; } From abc73b3467d20a4b4efce45e0c9125f84ff5293f Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 8 Nov 2023 01:23:47 +0100 Subject: [PATCH 12/40] ported mesh_msgs_conversions to ros2 humble --- mesh_msgs_conversions/CMakeLists.txt | 83 ++++++++---- mesh_msgs_conversions/COLCON_IGNORE | 0 mesh_msgs_conversions/README.md | 15 +++ .../mesh_msgs_conversions/conversions.h | 120 ++++++++--------- mesh_msgs_conversions/package.xml | 12 +- mesh_msgs_conversions/src/conversions.cpp | 122 ++++++++---------- 6 files changed, 191 insertions(+), 161 deletions(-) delete mode 100644 mesh_msgs_conversions/COLCON_IGNORE create mode 100644 mesh_msgs_conversions/README.md diff --git a/mesh_msgs_conversions/CMakeLists.txt b/mesh_msgs_conversions/CMakeLists.txt index 5dfe004..c28df55 100644 --- a/mesh_msgs_conversions/CMakeLists.txt +++ b/mesh_msgs_conversions/CMakeLists.txt @@ -1,14 +1,21 @@ -cmake_minimum_required(VERSION 3.1) - +cmake_minimum_required(VERSION 3.8) project(mesh_msgs_conversions) -set(PACKAGE_DEPENDENCIES - mesh_msgs - roscpp - sensor_msgs -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) find_package(LVR2 REQUIRED) find_package(OpenCV REQUIRED) find_package(MPI REQUIRED) @@ -16,8 +23,8 @@ find_package(PkgConfig REQUIRED) add_definitions(${LVR2_DEFINITIONS} ${OpenCV_DEFINITIONS}) -### compile with c++14 -set (CMAKE_CXX_STANDARD 14) + +find_library(LVR2_LIBRARY NAMES lvr2) # enable openmp support #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") @@ -29,33 +36,55 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) -catkin_package( - CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} - INCLUDE_DIRS include - DEPENDS LVR2 MPI - LIBRARIES ${PROJECT_NAME} -) - add_library(${PROJECT_NAME} src/conversions.cpp ) -find_library(LVR2_LIBRARY NAMES lvr2) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +# ament_target_dependencies(${PROJECT_NAME} +# rclcpp +# mesh_msgs +# sensor_msgs +# ${LVR2_LIBRARY} +# ${OpenCV_LIBRARIES} +# ) + +target_link_libraries(${PROJECT_NAME} PUBLIC ${LVR2_LIBRARY} ${OpenCV_LIBRARIES} + rclcpp::rclcpp + ${mesh_msgs_TARGETS} + ${sensor_msgs_TARGETS} ) -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_MSGS_CONVERSIONS_BUILDING_DLL") + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include ) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +ament_export_dependencies( + rclcpp + mesh_msgs + sensor_msgs +) + +ament_package() \ No newline at end of file diff --git a/mesh_msgs_conversions/COLCON_IGNORE b/mesh_msgs_conversions/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mesh_msgs_conversions/README.md b/mesh_msgs_conversions/README.md new file mode 100644 index 0000000..78a0d8b --- /dev/null +++ b/mesh_msgs_conversions/README.md @@ -0,0 +1,15 @@ +# mesh_msgs_conversions + + +Conversion functions between lvr2 and mesh_msgs. + + +## ROS2 Port TODOs + +No global nodes anymore + +I changed +1. ros::Time::now() -> rclcpp::Time() +2. ROS_INFO -> std::cout + +The first point could destroy functionallity. The second one could destroy the logging. TODO: do this correctly \ No newline at end of file diff --git a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h index 7cc7f23..c594a59 100644 --- a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h +++ b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h @@ -32,8 +32,7 @@ #include -#include -#include +#include "rclcpp/rclcpp.hpp" #include #include @@ -49,25 +48,25 @@ #include #include -#include -#include -#include +#include "std_msgs/msg/string.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/fill_image.hpp" -#include -#include -#include +#include "mesh_msgs/msg/mesh_face_cluster.hpp" +#include "mesh_msgs/msg/mesh_material.hpp" +#include "mesh_msgs/msg/mesh_triangle_indices.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "mesh_msgs/msg/mesh_geometry.hpp" +#include "mesh_msgs/msg/mesh_geometry_stamped.hpp" +#include "mesh_msgs/msg/mesh_materials_stamped.hpp" +#include "mesh_msgs/msg/mesh_vertex_colors.hpp" +#include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp" +#include "mesh_msgs/msg/mesh_vertex_costs_stamped.hpp" +#include "mesh_msgs/msg/mesh_vertex_tex_coords.hpp" +#include "mesh_msgs/msg/mesh_material.hpp" +#include "mesh_msgs/msg/mesh_texture.hpp" -#include +#include "sensor_msgs/point_cloud2_iterator.hpp" namespace mesh_msgs_conversions @@ -91,11 +90,11 @@ typedef boost::shared_ptr MaterialGroupPtr; template -inline const mesh_msgs::MeshGeometry toMeshGeometry( +inline const mesh_msgs::msg::MeshGeometry toMeshGeometry( const lvr2::HalfEdgeMesh>& hem, const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>()) { - mesh_msgs::MeshGeometry mesh_msg; + mesh_msgs::msg::MeshGeometry mesh_msg; mesh_msg.vertices.reserve(hem.numVertices()); mesh_msg.faces.reserve(hem.numFaces()); @@ -109,14 +108,14 @@ inline const mesh_msgs::MeshGeometry toMeshGeometry( { new_indices.insert(vH, k++); const auto& pi = hem.getVertexPosition(vH); - geometry_msgs::Point p; + geometry_msgs::msg::Point p; p.x = pi.x; p.y = pi.y; p.z = pi.z; mesh_msg.vertices.push_back(p); } for(auto fH : hem.faces()) { - mesh_msgs::MeshTriangleIndices indices; + mesh_msgs::msg::MeshTriangleIndices indices; auto vHs = hem.getVerticesOfFace(fH); indices.vertex_indices[0] = new_indices[vHs[0]]; indices.vertex_indices[1] = new_indices[vHs[1]]; @@ -127,7 +126,7 @@ inline const mesh_msgs::MeshGeometry toMeshGeometry( for(auto vH : hem.vertices()) { const auto& n = normals[vH]; - geometry_msgs::Point v; + geometry_msgs::msg::Point v; v.x = n.x; v.y = n.y; v.z = n.z; mesh_msg.vertex_normals.push_back(v); } @@ -136,14 +135,14 @@ inline const mesh_msgs::MeshGeometry toMeshGeometry( } template -inline const mesh_msgs::MeshGeometryStamped toMeshGeometryStamped( +inline const mesh_msgs::msg::MeshGeometryStamped toMeshGeometryStamped( const lvr2::HalfEdgeMesh>& hem, const std::string& frame_id, const std::string& uuid, const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>(), - const ros::Time& stamp = ros::Time::now()) + const rclcpp::Time& stamp = rclcpp::Time()) { - mesh_msgs::MeshGeometryStamped mesh_msg; + mesh_msgs::msg::MeshGeometryStamped mesh_msg; mesh_msg.mesh_geometry = toMeshGeometry(hem, normals); mesh_msg.uuid = uuid; mesh_msg.header.frame_id = frame_id; @@ -151,12 +150,12 @@ inline const mesh_msgs::MeshGeometryStamped toMeshGeometryStamped( return mesh_msg; } -inline const mesh_msgs::MeshVertexCosts toVertexCosts( +inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts( const lvr2::VertexMap& costs, const size_t num_values, const float default_value) { - mesh_msgs::MeshVertexCosts costs_msg; + mesh_msgs::msg::MeshVertexCosts costs_msg; costs_msg.costs.resize(num_values, default_value); for(auto vH : costs){ costs_msg.costs[vH.idx()] = costs[vH]; @@ -164,17 +163,17 @@ inline const mesh_msgs::MeshVertexCosts toVertexCosts( return costs_msg; } -inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( +inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped( const lvr2::VertexMap& costs, const size_t num_values, const float default_value, const std::string& name, const std::string& frame_id, const std::string& uuid, - const ros::Time& stamp = ros::Time::now() + const rclcpp::Time& stamp = rclcpp::Time() ) { - mesh_msgs::MeshVertexCostsStamped mesh_msg; + mesh_msgs::msg::MeshVertexCostsStamped mesh_msg; mesh_msg.mesh_vertex_costs = toVertexCosts(costs, num_values, default_value); mesh_msg.uuid = uuid; mesh_msg.type = name; @@ -183,9 +182,10 @@ inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( return mesh_msg; } -inline const mesh_msgs::MeshVertexCosts toVertexCosts(const lvr2::DenseVertexMap& costs) +inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts( + const lvr2::DenseVertexMap& costs) { - mesh_msgs::MeshVertexCosts costs_msg; + mesh_msgs::msg::MeshVertexCosts costs_msg; costs_msg.costs.reserve(costs.numValues()); for(auto vH : costs){ costs_msg.costs.push_back(costs[vH]); @@ -193,15 +193,15 @@ inline const mesh_msgs::MeshVertexCosts toVertexCosts(const lvr2::DenseVertexMap return costs_msg; } -inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( +inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped( const lvr2::DenseVertexMap& costs, const std::string& name, const std::string& frame_id, const std::string& uuid, - const ros::Time& stamp = ros::Time::now() + const rclcpp::Time& stamp = rclcpp::Time() ) { - mesh_msgs::MeshVertexCostsStamped mesh_msg; + mesh_msgs::msg::MeshVertexCostsStamped mesh_msg; mesh_msg.mesh_vertex_costs = toVertexCosts(costs); mesh_msg.uuid = uuid; mesh_msg.type = name; @@ -213,48 +213,32 @@ inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( bool fromMeshBufferToMeshGeometryMessage( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry + mesh_msgs::msg::MeshGeometry& mesh_geometry ); /// Convert lvr2::MeshBuffer to various messages for services bool fromMeshBufferToMeshMessages( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry, - mesh_msgs::MeshMaterials& mesh_materials, - mesh_msgs::MeshVertexColors& mesh_vertex_colors, - boost::optional&> texture_cache, + mesh_msgs::msg::MeshGeometry& mesh_geometry, + mesh_msgs::msg::MeshMaterials& mesh_materials, + mesh_msgs::msg::MeshVertexColors& mesh_vertex_colors, + boost::optional&> texture_cache, std::string mesh_uuid ); bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, + const std::shared_ptr mesh_geometry_ptr, lvr2::MeshBufferPtr& buffer_ptr ); bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer -); - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBufferPtr& buffer_ptr ); bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, - lvr2::MeshBufferPtr& buffer_ptr -); - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer -); - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, - lvr2::MeshBuffer& buffer -); + const mesh_msgs::msg::MeshGeometry& mesh_geometry, + lvr2::MeshBuffer& buffer); /* TODO void removeDuplicates(lvr2::MeshBuffer& buffer); @@ -277,7 +261,8 @@ bool readMeshBuffer(lvr2::MeshBufferPtr& buffer, string path); */ bool writeMeshBuffer(lvr2::MeshBufferPtr& mesh, string path); -bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, PointBuffer& buffer); +bool fromPointCloud2ToPointBuffer( + const sensor_msgs::msg::PointCloud2& cloud, PointBuffer& buffer); /** * @brief converts lvr2::Pointbuffer to pointcloud2. @@ -287,7 +272,10 @@ bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, PointBu * @param frame the frame of the converted pointcloud2 * @param cloud the converted pointcloud2 */ -void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string frame, sensor_msgs::PointCloud2Ptr& cloud); +void PointBufferToPointCloud2( + const lvr2::PointBufferPtr& buffer, + std::string frame, + std::shared_ptr& cloud); /** * @brief converts pointcloud2 to a newly created Pointerbuffer. @@ -298,7 +286,9 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr * * @return */ -void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::PointBufferPtr& buffer); +void PointCloud2ToPointBuffer( + const std::shared_ptr cloud, + lvr2::PointBufferPtr& buffer); /** @@ -308,7 +298,7 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po * @return bool success status */ bool fromMeshGeometryMessageToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, const lvr2::MeshBufferPtr& buffer ); diff --git a/mesh_msgs_conversions/package.xml b/mesh_msgs_conversions/package.xml index 2e05138..6f1f31a 100644 --- a/mesh_msgs_conversions/package.xml +++ b/mesh_msgs_conversions/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs_conversions 1.1.0 converts point clouds and attributes into meshes and mesh attributes @@ -10,11 +11,16 @@ Sebastian Pütz Henning Deeken + ament_cmake lvr2 - roscpp + rclcpp sensor_msgs mesh_msgs - catkin + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/mesh_msgs_conversions/src/conversions.cpp b/mesh_msgs_conversions/src/conversions.cpp index 760939e..16a9e65 100644 --- a/mesh_msgs_conversions/src/conversions.cpp +++ b/mesh_msgs_conversions/src/conversions.cpp @@ -36,12 +36,12 @@ namespace mesh_msgs_conversions bool fromMeshBufferToMeshGeometryMessage( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry + mesh_msgs::msg::MeshGeometry& mesh_geometry ){ size_t n_vertices = buffer->numVertices(); size_t n_faces = buffer->numFaces(); - ROS_DEBUG_STREAM("Copy vertices from MeshBuffer to MeshGeometry."); + // RCLCPP_DEBUG_STREAM("Copy vertices from MeshBuffer to MeshGeometry."); // Copy vertices mesh_geometry.vertices.resize(n_vertices); @@ -53,7 +53,7 @@ bool fromMeshBufferToMeshGeometryMessage( mesh_geometry.vertices[i].z = buffer_vertices[i * 3 + 2]; } - ROS_DEBUG_STREAM("Copy faces from MeshBuffer to MeshGeometry."); + // RCLCPP_DEBUG_STREAM("Copy faces from MeshBuffer to MeshGeometry."); // Copy faces auto buffer_faces = buffer->getFaceIndices(); @@ -69,7 +69,7 @@ bool fromMeshBufferToMeshGeometryMessage( auto buffer_vertexnormals = buffer->getVertexNormals(); if(buffer->hasVertexNormals()) { - ROS_DEBUG_STREAM("Copy normals from MeshBuffer to MeshGeometry."); + // RCLCPP_DEBUG_STREAM("Copy normals from MeshBuffer to MeshGeometry."); mesh_geometry.vertex_normals.resize(n_vertices); for (unsigned int i = 0; i < n_vertices; i++) { @@ -78,20 +78,20 @@ bool fromMeshBufferToMeshGeometryMessage( mesh_geometry.vertex_normals[i].z = buffer_vertexnormals[i * 3 + 2]; } }else{ - ROS_DEBUG_STREAM("No vertex normals given!"); + // RCLCPP_DEBUG_STREAM("No vertex normals given!"); } - ROS_DEBUG_STREAM("Successfully copied the MeshBuffer " - "geometry to the MeshGeometry message."); + // RCLCPP_DEBUG_STREAM("Successfully copied the MeshBuffer " + // "geometry to the MeshGeometry message."); return true; } bool fromMeshBufferToMeshMessages( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry, - mesh_msgs::MeshMaterials& mesh_materials, - mesh_msgs::MeshVertexColors& mesh_vertex_colors, - boost::optional&> texture_cache, + mesh_msgs::msg::MeshGeometry& mesh_geometry, + mesh_msgs::msg::MeshMaterials& mesh_materials, + mesh_msgs::msg::MeshVertexColors& mesh_vertex_colors, + boost::optional&> texture_cache, std::string mesh_uuid ) { @@ -198,7 +198,7 @@ bool fromMeshBufferToMeshMessages( texture_cache.get().resize(n_textures); for (unsigned int i = 0; i < n_textures; i++) { - sensor_msgs::Image image; + sensor_msgs::msg::Image image; sensor_msgs::fillImage( image, "rgb8", @@ -207,7 +207,7 @@ bool fromMeshBufferToMeshMessages( buffer_textures[i].m_width * 3, // step size buffer_textures[i].m_data ); - mesh_msgs::MeshTexture texture; + mesh_msgs::msg::MeshTexture texture; texture.uuid = mesh_uuid; texture.texture_index = i; texture.image = image; @@ -220,22 +220,7 @@ bool fromMeshBufferToMeshMessages( } bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer) -{ - return fromMeshGeometryToMeshBuffer(*mesh_geometry_ptr, buffer); -} - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, - lvr2::MeshBufferPtr& buffer_ptr) -{ - if(!buffer_ptr) buffer_ptr = lvr2::MeshBufferPtr(new lvr2::MeshBuffer); - return fromMeshGeometryToMeshBuffer(*mesh_geometry_ptr, *buffer_ptr); -} - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, + const std::shared_ptr mesh_geometry_ptr, lvr2::MeshBufferPtr& buffer_ptr) { if(!buffer_ptr) buffer_ptr = lvr2::MeshBufferPtr(new lvr2::MeshBuffer); @@ -243,14 +228,7 @@ bool fromMeshGeometryToMeshBuffer( } bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer) -{ - return fromMeshGeometryToMeshBuffer(*mesh_geometry_ptr, buffer); -} - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBufferPtr& buffer_ptr) { if(!buffer_ptr) buffer_ptr = lvr2::MeshBufferPtr(new lvr2::MeshBuffer); @@ -258,7 +236,7 @@ bool fromMeshGeometryToMeshBuffer( } bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBuffer& buffer) { @@ -372,7 +350,9 @@ void removeDuplicates(lvr2::MeshBuffer& buffer) } */ -static inline bool hasCloudChannel(const sensor_msgs::PointCloud2& cloud, const std::string& field_name) +static inline bool hasCloudChannel( + const sensor_msgs::msg::PointCloud2& cloud, + const std::string& field_name) { // Get the index we need for (size_t d = 0; d < cloud.fields.size(); ++d) @@ -381,7 +361,9 @@ static inline bool hasCloudChannel(const sensor_msgs::PointCloud2& cloud, const return false; } -bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, lvr2::PointBuffer& buffer) +bool fromPointCloud2ToPointBuffer( + const sensor_msgs::msg::PointCloud2& cloud, + lvr2::PointBuffer& buffer) { size_t size = cloud.height * cloud.width; @@ -536,7 +518,7 @@ bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, lvr2::P } bool fromMeshGeometryMessageToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, const lvr2::MeshBufferPtr& buffer ) { @@ -580,20 +562,26 @@ bool fromMeshGeometryMessageToMeshBuffer( } else { - ROS_ERROR_STREAM("Number of normals (" << mesh_geometry.vertex_normals.size() + // RCLCPP_ERROR_STREAM("Number of normals (" << mesh_geometry.vertex_normals.size() + // << ") must be equal to number of vertices (" << mesh_geometry.vertices.size() + // << "), ignore normals!"); + std::cerr << "Number of normals (" << mesh_geometry.vertex_normals.size() << ") must be equal to number of vertices (" << mesh_geometry.vertices.size() - << "), ignore normals!"); + << "), ignore normals!" << std::endl; } return true; } -void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string frame, sensor_msgs::PointCloud2Ptr& cloud) +void PointBufferToPointCloud2( + const lvr2::PointBufferPtr& buffer, + std::string frame, + std::shared_ptr& cloud) { // the offset will be updated by addPointField - cloud->header.stamp = ros::Time::now(); + cloud->header.stamp = rclcpp::Time(); cloud->header.frame_id = frame; - ros::Rate r(60); +// ros::Rate r(60); int type; std::map > floatChannels; @@ -613,9 +601,9 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr { size = channelPair.second.numElements(); int p_offset = 0; - p_offset = addPointField(*cloud, "x", 1, sensor_msgs::PointField::FLOAT32, p_offset); - p_offset = addPointField(*cloud, "y", 1, sensor_msgs::PointField::FLOAT32, p_offset); - p_offset = addPointField(*cloud, "z", 1, sensor_msgs::PointField::FLOAT32, p_offset); + p_offset = addPointField(*cloud, "x", 1, sensor_msgs::msg::PointField::FLOAT32, p_offset); + p_offset = addPointField(*cloud, "y", 1, sensor_msgs::msg::PointField::FLOAT32, p_offset); + p_offset = addPointField(*cloud, "z", 1, sensor_msgs::msg::PointField::FLOAT32, p_offset); p_offset += sizeof(float); cloud->point_step = offset; for(auto channelPair: uCharChannels) @@ -623,7 +611,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr if(channelPair.first == "colors") { - offset = addPointField(*cloud, "rgb", channelPair.second.width(), sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(*cloud, "rgb", channelPair.second.width(), sensor_msgs::msg::PointField::FLOAT32, offset); cloud->point_step = offset; } } @@ -668,7 +656,8 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr cloud->width = size; - ROS_INFO("Starting conversion."); +// RCLCPP_INFO("Starting conversion."); + std::cout << "Starting conversion." << std::endl; for(auto field: cloud->fields) { // Points is a special case... @@ -678,7 +667,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr //auto iter_x = floatIters.at("x"); //auto iter_y = floatIters.at("y"); //auto iter_z = floatIters.at("z"); - #pragma omp parallel for + for(size_t i = 0; i < size; ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]); @@ -693,7 +682,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr } else { - if(field.datatype == sensor_msgs::PointField::FLOAT32) + if(field.datatype == sensor_msgs::msg::PointField::FLOAT32) { std::string name = field.name; if(name == "rgb") @@ -701,7 +690,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr name = "colors"; auto channel = uCharChannels.at(name); //auto iter = uCharIters.at(field.name); -#pragma omp parallel for + for(size_t i = 0; i < size; ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; @@ -720,7 +709,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr // ELSE auto channel = floatChannels.at(field.name); //auto iter = floatIters.at(field.name); - #pragma omp parallel for + for(size_t i = 0; i < size; ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; @@ -731,21 +720,23 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr } } } - else if (field.datatype == sensor_msgs::PointField::UINT8) + else if (field.datatype == sensor_msgs::msg::PointField::UINT8) { } } } - ROS_INFO("DONE"); +// RCLCPP_INFO("DONE"); } -void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::PointBufferPtr& buffer) +void PointCloud2ToPointBuffer( + const std::shared_ptr cloud, + lvr2::PointBufferPtr& buffer) { buffer = lvr2::PointBufferPtr(new lvr2::PointBuffer()); for(auto field : cloud->fields) { - if(field.datatype == sensor_msgs::PointField::FLOAT32) + if(field.datatype == sensor_msgs::msg::PointField::FLOAT32) { if(field.name == "x" || field.name == "y" || field.name == "z") { @@ -778,7 +769,6 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po buffer->setPointArray(points, cloud->width * cloud->height); channel = buffer->getChannel("points"); - #pragma omp parallel for for(size_t i = 0; i < (cloud->width * cloud->height); ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]); @@ -790,17 +780,17 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po } else { - if(field.datatype == sensor_msgs::PointField::FLOAT32) + if(field.datatype == sensor_msgs::msg::PointField::FLOAT32) { auto channel = buffer->getChannel(field.name); if(!channel) { - ROS_INFO("Channel %s missing", field.name.c_str()); + std::cout << "Channel " << field.name << " missing" << std::endl; + // RCLCPP_INFO("Channel %s missing", field.name.c_str()); continue; } - #pragma omp parallel for for(size_t i = 0; i < (cloud->width * cloud->height); ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; @@ -810,17 +800,17 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po } } } - else if (field.datatype == sensor_msgs::PointField::UINT8) + else if (field.datatype == sensor_msgs::msg::PointField::UINT8) { auto channel = buffer->getChannel(field.name); if(!channel) { - ROS_INFO("Channel %s missing", field.name.c_str()); + // RCLCPP_INFO("Channel %s missing", field.name.c_str()); + std::cout << "Channel " << field.name << " missing" << std::endl; continue; } - #pragma omp parallel for for(size_t i = 0; i < (cloud->width * cloud->height); ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; From 762063540cf4e180eecaa669a83c921529620f96 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 8 Nov 2023 01:28:53 +0100 Subject: [PATCH 13/40] added readmes and todos --- mesh_msgs_conversions/CMakeLists.txt | 8 -------- mesh_msgs_transform/CMakeLists.txt | 14 +++++++++++++- mesh_msgs_transform/README.md | 7 +++++++ 3 files changed, 20 insertions(+), 9 deletions(-) create mode 100644 mesh_msgs_transform/README.md diff --git a/mesh_msgs_conversions/CMakeLists.txt b/mesh_msgs_conversions/CMakeLists.txt index c28df55..9e4a931 100644 --- a/mesh_msgs_conversions/CMakeLists.txt +++ b/mesh_msgs_conversions/CMakeLists.txt @@ -44,14 +44,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") -# ament_target_dependencies(${PROJECT_NAME} -# rclcpp -# mesh_msgs -# sensor_msgs -# ${LVR2_LIBRARY} -# ${OpenCV_LIBRARIES} -# ) - target_link_libraries(${PROJECT_NAME} PUBLIC ${LVR2_LIBRARY} ${OpenCV_LIBRARIES} diff --git a/mesh_msgs_transform/CMakeLists.txt b/mesh_msgs_transform/CMakeLists.txt index a3aa19a..f1681df 100644 --- a/mesh_msgs_transform/CMakeLists.txt +++ b/mesh_msgs_transform/CMakeLists.txt @@ -1,6 +1,18 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(mesh_msgs_transform) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) + find_package(catkin REQUIRED COMPONENTS tf geometry_msgs diff --git a/mesh_msgs_transform/README.md b/mesh_msgs_transform/README.md new file mode 100644 index 0000000..6adaf25 --- /dev/null +++ b/mesh_msgs_transform/README.md @@ -0,0 +1,7 @@ +# mesh_msgs_transform + +Functions for transforming meshes. + +## ROS2 Port TODOs + +Here is still tf(1) used. Is this package in use? \ No newline at end of file From 32a2ae49d7b0042c4b2539a3a57341f2e7f8094b Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 8 Nov 2023 02:46:57 +0100 Subject: [PATCH 14/40] started to port rviz_map_plugin --- rviz_map_plugin/CMakeLists.txt | 81 ++++++------ .../include/ClusterLabelDisplay.hpp | 20 +-- rviz_map_plugin/include/ClusterLabelPanel.hpp | 16 +-- rviz_map_plugin/include/ClusterLabelTool.hpp | 40 +++--- .../include/ClusterLabelVisual.hpp | 14 +-- rviz_map_plugin/include/MapDisplay.hpp | 56 ++++----- rviz_map_plugin/include/MeshDisplay.hpp | 82 ++++++------ rviz_map_plugin/include/MeshGoalTool.hpp | 6 +- rviz_map_plugin/include/MeshPoseTool.hpp | 8 +- rviz_map_plugin/include/MeshVisual.hpp | 34 ++--- rviz_map_plugin/include/RvizFileProperty.hpp | 2 +- rviz_map_plugin/package.xml | 41 +++--- rviz_map_plugin/rviz_plugin.xml | 12 +- rviz_map_plugin/src/ClusterLabelDisplay.cpp | 22 ++-- rviz_map_plugin/src/ClusterLabelPanel.cpp | 14 +-- rviz_map_plugin/src/ClusterLabelTool.cpp | 14 +-- rviz_map_plugin/src/ClusterLabelVisual.cpp | 6 +- rviz_map_plugin/src/MapDisplay.cpp | 34 ++--- rviz_map_plugin/src/MeshDisplay.cpp | 119 +++++++++--------- rviz_map_plugin/src/MeshGoalTool.cpp | 6 +- rviz_map_plugin/src/MeshPoseTool.cpp | 14 +-- rviz_map_plugin/src/MeshVisual.cpp | 2 +- 22 files changed, 326 insertions(+), 317 deletions(-) diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index 90b90fa..0d02242 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -1,21 +1,30 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(rviz_map_plugin) -set(THIS_PACKAGE_ROS_DEPS - roscpp - rviz - std_msgs - mesh_msgs - hdf5_map_io -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(std_msgs REQUIRED) +find_package(hdf5_map_io REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) -find_package(catkin REQUIRED COMPONENTS - ${THIS_PACKAGE_ROS_DEPS} -) find_package(Boost REQUIRED COMPONENTS system) find_package(HDF5 REQUIRED COMPONENTS C CXX HL) -find_package(OpenCL 2 REQUIRED) +find_package(OpenCL 2) + find_package(assimp) @@ -24,12 +33,6 @@ if(assimp_FOUND) add_definitions(-DWITH_ASSIMP) endif(assimp_FOUND) - -catkin_package( - CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} - DEPENDS Boost OpenCL HDF5 OpenCL -) - include_directories( include ${catkin_INCLUDE_DIRS} @@ -41,17 +44,10 @@ include_directories( ## This setting causes Qt's "MOC" generation to happen automatically. set(CMAKE_AUTOMOC ON) -if("${rviz_QT_VERSION}" VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - ## pull in all required include dirs, define QT_LIBRARIES, etc. - include(${QT_USE_FILE}) -else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies - set(QT_LIBRARIES Qt5::Widgets) -endif() +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies +set(QT_LIBRARIES Qt5::Widgets) + add_definitions(-DQT_NO_KEYWORDS) @@ -89,26 +85,29 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} - ${OpenCL_LIBRARIES} ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + mesh_msgs + rviz_common + rviz_rendering + tf2_ros + pluginlib ) + install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) install(FILES rviz_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME}) install(DIRECTORY icons/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) + DESTINATION share/${PROJECT_NAME}/icons) -get_property(dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) -foreach(dir ${dirs}) - message(STATUS "dir='${dir}'") -endforeach() +ament_package() \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelDisplay.hpp b/rviz_map_plugin/include/ClusterLabelDisplay.hpp index 8d3ac4c..040cde0 100644 --- a/rviz_map_plugin/include/ClusterLabelDisplay.hpp +++ b/rviz_map_plugin/include/ClusterLabelDisplay.hpp @@ -67,14 +67,14 @@ #include #include -#include -#include +// #include +// #include #include #include #include #include -#include +#include #include #include @@ -133,7 +133,7 @@ class ClusterLabelTool; * @class ClusterLabelDisplay * @brief Display class for the map plugin */ -class ClusterLabelDisplay : public rviz::Display +class ClusterLabelDisplay : public rviz_common::Display { Q_OBJECT @@ -266,22 +266,22 @@ private Q_SLOTS: ClusterLabelTool* m_tool; /// Property for the current active visual - rviz::EnumProperty* m_activeVisualProperty; + rviz_common::EnumProperty* m_activeVisualProperty; /// Property to set transparency - rviz::FloatProperty* m_alphaProperty; + rviz_common::FloatProperty* m_alphaProperty; /// Property for selecting colors (menu) - rviz::Property* m_colorsProperty; + rviz_common::Property* m_colorsProperty; /// Properties for selecting colors (menu-items) - std::vector m_colorProperties; + std::vector m_colorProperties; /// Property to set the brushsize of the sphere brush of the label tool from this package - rviz::FloatProperty* m_sphereSizeProperty; + rviz_common::FloatProperty* m_sphereSizeProperty; /// Property to hide or show a phantom visual - rviz::BoolProperty* m_phantomVisualProperty; + rviz_common::BoolProperty* m_phantomVisualProperty; /// Index for the visuals int m_labelToolVisualIndex = 0; diff --git a/rviz_map_plugin/include/ClusterLabelPanel.hpp b/rviz_map_plugin/include/ClusterLabelPanel.hpp index a10ca7e..6924ba2 100644 --- a/rviz_map_plugin/include/ClusterLabelPanel.hpp +++ b/rviz_map_plugin/include/ClusterLabelPanel.hpp @@ -50,12 +50,12 @@ #define CLUSTER_LABEL_PANEL_HPP #include -#include +// #include #include -#include -#include +#include +#include #include -#include +#include // Forward declarations class QLineEdit; @@ -72,7 +72,7 @@ namespace rviz_map_plugin * @class ClusterLabelPanel * @brief Panel for interacting with the label tool */ -class ClusterLabelPanel : public rviz::Panel +class ClusterLabelPanel : public rviz_common::Panel { Q_OBJECT @@ -92,13 +92,13 @@ class ClusterLabelPanel : public rviz::Panel * @brief Load a configuration * @input config The configuration */ - virtual void load(const rviz::Config& config); + virtual void load(const rviz_common::Config& config); /** * @brief Save a configuration * @input config The configuration */ - virtual void save(rviz::Config config) const; + virtual void save(rviz_common::Config config) const; public Q_SLOTS: @@ -141,7 +141,7 @@ public Q_SLOTS: ClusterLabelTool* m_tool; /// Node handle - ros::NodeHandle m_nodeHandle; + // ros::NodeHandle m_nodeHandle; }; } // end namespace rviz_map_plugin diff --git a/rviz_map_plugin/include/ClusterLabelTool.hpp b/rviz_map_plugin/include/ClusterLabelTool.hpp index 8deb6af..fb4508d 100644 --- a/rviz_map_plugin/include/ClusterLabelTool.hpp +++ b/rviz_map_plugin/include/ClusterLabelTool.hpp @@ -70,23 +70,23 @@ #include #include -#include -#include -#include -#include +// #include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include +#include #ifndef Q_MOC_RUN -#include +#include #include #include @@ -122,7 +122,7 @@ class ClusterLabelVisual; * @class ClusterLabelTool * @brief Tool for selecting faces */ -class ClusterLabelTool : public rviz::Tool +class ClusterLabelTool : public rviz_common::Tool { Q_OBJECT public: @@ -156,7 +156,7 @@ class ClusterLabelTool : public rviz::Tool * @param event The mouse event * @return Exit code */ - virtual int processMouseEvent(rviz::ViewportMouseEvent& event); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event); /** * @brief Connects this tool with a given display @@ -210,7 +210,7 @@ public Q_SLOTS: float m_sphereSize = 1.0f; // Selection Box - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; Ogre::SceneNode* m_sceneNode; Ogre::ManualObject* m_selectionBox; Ogre::MaterialPtr m_selectionBoxMaterial; @@ -223,13 +223,13 @@ public Q_SLOTS: std::vector m_vertexPositions; void updateSelectionBox(); - void selectionBoxStart(rviz::ViewportMouseEvent& event); - void selectionBoxMove(rviz::ViewportMouseEvent& event); - void selectMultipleFaces(rviz::ViewportMouseEvent& event, bool selectMode); + 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::ViewportMouseEvent& event, bool selectMode); + void selectSingleFace(rviz_common::ViewportMouseEvent& event, bool selectMode); void selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode); - void selectSphereFaces(rviz::ViewportMouseEvent& event, bool selectMode); + void selectSphereFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); void selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode); boost::optional> getClosestIntersectedFaceParallel(Ogre::Ray& ray); diff --git a/rviz_map_plugin/include/ClusterLabelVisual.hpp b/rviz_map_plugin/include/ClusterLabelVisual.hpp index c21f13e..9af4778 100644 --- a/rviz_map_plugin/include/ClusterLabelVisual.hpp +++ b/rviz_map_plugin/include/ClusterLabelVisual.hpp @@ -50,12 +50,12 @@ #include -#include +// #include -#include +#include -#include -#include +#include +#include #include #include #include @@ -86,7 +86,7 @@ class ClusterLabelVisual * @param context The context that contains the display information. * @param labelId The label id (that has to be unique) */ - ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId); + ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId); /** * @brief Constructor @@ -95,7 +95,7 @@ class ClusterLabelVisual * @param labelId The label id (that has to be unique) * @param geometry A shared pointer to the geometry to which the labels belong */ - ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId, std::shared_ptr geometry); + ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId, std::shared_ptr geometry); /** * @brief Destructor @@ -158,7 +158,7 @@ class ClusterLabelVisual private: void initMaterial(); - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; Ogre::SceneNode* m_sceneNode; std::string m_labelId; diff --git a/rviz_map_plugin/include/MapDisplay.hpp b/rviz_map_plugin/include/MapDisplay.hpp index 83dab27..2df81c7 100644 --- a/rviz_map_plugin/include/MapDisplay.hpp +++ b/rviz_map_plugin/include/MapDisplay.hpp @@ -69,34 +69,34 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +// #include +// #include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include #include #ifndef Q_MOC_RUN -#include +#include #include #include @@ -136,7 +136,7 @@ using std::vector; * @class MapDisplay * @brief Master display for the Mesh- and Cluster- subdisplays. THis implementation uses HDF5 as it's data source */ -class MapDisplay : public rviz::Display +class MapDisplay : public rviz_common::Display { Q_OBJECT @@ -151,7 +151,7 @@ class MapDisplay : public rviz::Display */ ~MapDisplay(); - virtual void load(const rviz::Config& config); + virtual void load(const rviz_common::Config& config); public Q_SLOTS: @@ -218,7 +218,7 @@ private Q_SLOTS: std::shared_ptr m_nh_p; /// Path to map file - rviz::FileProperty* m_mapFilePath; + rviz_common::FileProperty* m_mapFilePath; std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) @@ -231,7 +231,7 @@ private Q_SLOTS: * @param class_id The class ID * @return Pointer to RViz display */ - rviz::Display* createDisplay(const QString& class_id); + rviz_common::Display* createDisplay(const QString& class_id); }; } // end namespace rviz_map_plugin diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_map_plugin/include/MeshDisplay.hpp index 9a85972..c745814 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_map_plugin/include/MeshDisplay.hpp @@ -69,25 +69,25 @@ #include #include -#include -#include -#include -#include -#include -#include +// #include +// #include +#include +#include +#include +#include -#include -#include -#include +#include +#include +#include #ifndef Q_MOC_RUN -#include -#include -#include +// #include +// #include +// #include #include -#include +#include #include #include @@ -98,7 +98,9 @@ #endif -namespace rviz +namespace rviz_common +{ +namespace properties { // Forward declaration class BoolProperty; @@ -108,8 +110,8 @@ class IntProperty; class RosTopicProperty; class EnumProperty; class StringProperty; - -} // End namespace rviz +} // end namespace properties +} // end namespace rviz_common namespace rviz_map_plugin { @@ -125,7 +127,7 @@ class MeshVisual; * @class MeshDisplay * @brief Display for showing the mesh in different modes */ -class MeshDisplay : public rviz::Display +class MeshDisplay : public rviz_common::Display { Q_OBJECT @@ -401,73 +403,73 @@ private Q_SLOTS: std::queue> m_visuals; /// Property to handle topic for meshMsg - rviz::RosTopicProperty* m_meshTopic; + rviz_common::properties::RosTopicProperty* m_meshTopic; /// Property to handle buffer size - rviz::IntProperty* m_bufferSize; + rviz_common::properties::IntProperty* m_bufferSize; /// Property to select the display type - rviz::EnumProperty* m_displayType; + rviz_common::properties::EnumProperty* m_displayType; /// Property to set faces color - rviz::ColorProperty* m_facesColor; + rviz_common::properties::ColorProperty* m_facesColor; /// Property to set faces transparency - rviz::FloatProperty* m_facesAlpha; + rviz_common::properties::FloatProperty* m_facesAlpha; /// Property to handle topic for vertex colors - rviz::RosTopicProperty* m_vertexColorsTopic; + rviz_common::properties::RosTopicProperty* m_vertexColorsTopic; /// Property to handle service name for vertexColors - rviz::StringProperty* m_vertexColorServiceName; + rviz_common::properties::StringProperty* m_vertexColorServiceName; /// Property to only show textured faces when texturizing is enabled - rviz::BoolProperty* m_showTexturedFacesOnly; + rviz_common::properties::BoolProperty* m_showTexturedFacesOnly; /// Property to handle service name for materials - rviz::StringProperty* m_materialServiceName; + rviz_common::properties::StringProperty* m_materialServiceName; /// Property to handle service name for textures - rviz::StringProperty* m_textureServiceName; + rviz_common::properties::StringProperty* m_textureServiceName; /// Property for selecting the color type for cost display - rviz::EnumProperty* m_costColorType; + rviz_common::properties::EnumProperty* m_costColorType; /// Property to handle topic for vertex cost maps - rviz::RosTopicProperty* m_vertexCostsTopic; + rviz_common::properties::RosTopicProperty* m_vertexCostsTopic; /// Property to select different types of vertex cost maps to be shown - rviz::EnumProperty* m_selectVertexCostMap; + rviz_common::properties::EnumProperty* m_selectVertexCostMap; /// Property for using custom limits for cost display - rviz::BoolProperty* m_costUseCustomLimits; + rviz_common::properties::BoolProperty* m_costUseCustomLimits; /// Property for setting the lower limit of cost display - rviz::FloatProperty* m_costLowerLimit; + rviz_common::properties::FloatProperty* m_costLowerLimit; /// Property for setting the upper limit of cost display - rviz::FloatProperty* m_costUpperLimit; + rviz_common::properties::FloatProperty* m_costUpperLimit; /// Property to select the normals - rviz::BoolProperty* m_showNormals; + rviz_common::properties::BoolProperty* m_showNormals; /// Property to set the color of the normals - rviz::ColorProperty* m_normalsColor; + rviz_common::properties::ColorProperty* m_normalsColor; /// Property to set the transparency of the normals - rviz::FloatProperty* m_normalsAlpha; + rviz_common::properties::FloatProperty* m_normalsAlpha; /// Property to set the size of the normals - rviz::FloatProperty* m_scalingFactor; + rviz_common::properties::FloatProperty* m_scalingFactor; /// Property to select the wireframe - rviz::BoolProperty* m_showWireframe; + rviz_common::properties::BoolProperty* m_showWireframe; /// Property to set wireframe color - rviz::ColorProperty* m_wireframeColor; + rviz_common::properties::ColorProperty* m_wireframeColor; /// Property to set wireframe transparency - rviz::FloatProperty* m_wireframeAlpha; + rviz_common::properties::FloatProperty* m_wireframeAlpha; /// Cache for received vertex cost messages std::map> m_costCache; diff --git a/rviz_map_plugin/include/MeshGoalTool.hpp b/rviz_map_plugin/include/MeshGoalTool.hpp index cff9695..cc96cc0 100644 --- a/rviz_map_plugin/include/MeshGoalTool.hpp +++ b/rviz_map_plugin/include/MeshGoalTool.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #ifndef Q_MOC_RUN #include @@ -91,9 +91,9 @@ private Q_SLOTS: virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); /// Property for the topic - rviz::StringProperty* topic_property_; + rviz_common::StringProperty* topic_property_; /// Switch bottom / top for selection - rviz::BoolProperty* switch_bottom_top_; + rviz_common::BoolProperty* switch_bottom_top_; /// Publisher ros::Publisher pose_pub_; /// Node handle diff --git a/rviz_map_plugin/include/MeshPoseTool.hpp b/rviz_map_plugin/include/MeshPoseTool.hpp index 0c0b62c..dc3eba1 100644 --- a/rviz_map_plugin/include/MeshPoseTool.hpp +++ b/rviz_map_plugin/include/MeshPoseTool.hpp @@ -57,7 +57,7 @@ namespace rviz_map_plugin { -class MeshPoseTool : public rviz::Tool +class MeshPoseTool : public rviz_common::Tool { public: MeshPoseTool(); @@ -68,7 +68,7 @@ class MeshPoseTool : public rviz::Tool virtual void activate(); virtual void deactivate(); - virtual int processMouseEvent(rviz::ViewportMouseEvent& event); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event); protected: virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) = 0; @@ -79,9 +79,9 @@ class MeshPoseTool : public rviz::Tool bool getPositionAndOrientation(const Ogre::ManualObject* mesh, const Ogre::Ray& ray, Ogre::Vector3& position, Ogre::Vector3& orientation); - bool selectTriangle(rviz::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& orientation); + bool selectTriangle(rviz_common::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& orientation); - rviz::Arrow* arrow_; + rviz_common::Arrow* arrow_; enum State { Position, diff --git a/rviz_map_plugin/include/MeshVisual.hpp b/rviz_map_plugin/include/MeshVisual.hpp index c191e1e..1627f6b 100644 --- a/rviz_map_plugin/include/MeshVisual.hpp +++ b/rviz_map_plugin/include/MeshVisual.hpp @@ -53,21 +53,21 @@ #ifndef MESH_VISUAL_HPP #define MESH_VISUAL_HPP -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include #include #include @@ -105,7 +105,7 @@ class MeshVisual * @param meshID The mesh id * @param randomID random number that will be used as part of the meshes UID */ - MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID); + MeshVisual(rviz_common::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID); /** * @brief Destructor. @@ -321,7 +321,7 @@ class MeshVisual Ogre::SceneNode* m_sceneNode; /// The context that contains the display information. - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; /// First ID of the created mesh size_t m_prefix; diff --git a/rviz_map_plugin/include/RvizFileProperty.hpp b/rviz_map_plugin/include/RvizFileProperty.hpp index 5540b9c..8f254b0 100644 --- a/rviz_map_plugin/include/RvizFileProperty.hpp +++ b/rviz_map_plugin/include/RvizFileProperty.hpp @@ -49,7 +49,7 @@ #define RVIZ_FILE_PROPERTY_HPP #include -#include +#include namespace rviz { diff --git a/rviz_map_plugin/package.xml b/rviz_map_plugin/package.xml index 6959b4c..a26e626 100644 --- a/rviz_map_plugin/package.xml +++ b/rviz_map_plugin/package.xml @@ -1,4 +1,6 @@ - + + + rviz_map_plugin RViz display types and tools for the mesh_msgs package. @@ -13,25 +15,26 @@ BSD-3 https://github.com/uos/mesh_tools/tree/master/rviz_map_plugin - qtbase5-dev - roscpp - rviz - std_msgs - mesh_msgs - hdf5_map_io - ocl-icd-opencl-dev - opencl-headers - libqt5-core - libqt5-gui - libqt5-widgets - roscpp - rviz - std_msgs - mesh_msgs - hdf5_map_io - ocl-icd-opencl-dev - opencl-headers + + ament_cmake + + qtbase5-dev + rclcpp + rviz_common + rviz_rendering + std_msgs + mesh_msgs + hdf5_map_io + tf2_ros + pluginlib + ocl-icd-opencl-dev + opencl-headers + libqt5-core + libqt5-gui + libqt5-widgets + + ament_cmake diff --git a/rviz_map_plugin/rviz_plugin.xml b/rviz_map_plugin/rviz_plugin.xml index 455f46c..8da02e8 100644 --- a/rviz_map_plugin/rviz_plugin.xml +++ b/rviz_map_plugin/rviz_plugin.xml @@ -1,42 +1,42 @@ + base_class_type="rviz_common::Panel"> A panel widget allowing creation of cluster labels. + base_class_type="rviz_common::Tool"> A tool allowing selection of clusters. + base_class_type="rviz_common::Display"> Displays labeled clusters of a map. (Don't use without Map3D) + base_class_type="rviz_common::Display"> Displays a map with labeled clusters. + base_class_type="rviz_common::Display"> Displays a mesh. + base_class_type="rviz_common::Tool"> Select a goal on a mesh. diff --git a/rviz_map_plugin/src/ClusterLabelDisplay.cpp b/rviz_map_plugin/src/ClusterLabelDisplay.cpp index 356dc71..f3c3d0a 100644 --- a/rviz_map_plugin/src/ClusterLabelDisplay.cpp +++ b/rviz_map_plugin/src/ClusterLabelDisplay.cpp @@ -92,25 +92,25 @@ Ogre::ColourValue getRainbowColor(float value) ClusterLabelDisplay::ClusterLabelDisplay() { m_activeVisualProperty = - new rviz::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", + new rviz_common::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", this, SLOT(changeVisual()), this); - m_alphaProperty = new rviz::FloatProperty("Transparency", 1.0f, + m_alphaProperty = new rviz_common::FloatProperty("Transparency", 1.0f, "Transparency of the Labeled Cluster Visualization. 0.0 is fully " "transparent, 1.0 fully opaque", this, SLOT(updateColors()), this); m_alphaProperty->setMin(0.0f); m_alphaProperty->setMax(1.0f); - m_colorsProperty = new rviz::Property("Colors", "", "colors", this, SLOT(updateColors()), this); + m_colorsProperty = new rviz_common::Property("Colors", "", "colors", this, SLOT(updateColors()), this); m_colorsProperty->setReadOnly(true); m_sphereSizeProperty = - new rviz::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); - m_phantomVisualProperty = new rviz::BoolProperty("Show Phantom", false, + new rviz_common::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); + m_phantomVisualProperty = new rviz_common::BoolProperty("Show Phantom", false, "Show a transparent silhouette of the whole mesh to help with " "labeling", this, SLOT(updatePhantomVisual()), this); - setStatus(rviz::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); + setStatus(rviz_common::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); } ClusterLabelDisplay::~ClusterLabelDisplay() @@ -149,7 +149,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector if (isEnabled()) updateMap(); - setStatus(rviz::StatusProperty::Ok, "Display", ""); + setStatus(rviz_common::StatusProperty::Ok, "Display", ""); } // ===================================================================================================================== @@ -224,7 +224,7 @@ void ClusterLabelDisplay::updateMap() m_tool->setDisplay(this); // All good - setStatus(rviz::StatusProperty::Ok, "Map", ""); + setStatus(rviz_common::StatusProperty::Ok, "Map", ""); } void ClusterLabelDisplay::updateColors() @@ -267,7 +267,7 @@ void ClusterLabelDisplay::fillPropertyOptions() // Add color options Ogre::ColourValue rainbowColor = getRainbowColor((((float)i + 1) / m_clusterList.size())); - m_colorProperties.emplace_back(new rviz::ColorProperty( + m_colorProperties.emplace_back(new rviz_common::ColorProperty( QString::fromStdString(m_clusterList[i].name), QColor(rainbowColor.r * 255, rainbowColor.g * 255, rainbowColor.b * 255), QString::fromStdString(m_clusterList[i].name), m_colorsProperty, SLOT(updateColors()), this)); @@ -318,7 +318,7 @@ void ClusterLabelDisplay::createPhantomVisual() void ClusterLabelDisplay::initializeLabelTool() { // Check if the cluster label tool is already opened - rviz::ToolManager* toolManager = context_->getToolManager(); + rviz_common::ToolManager* toolManager = context_->getToolManager(); QStringList toolClasses = toolManager->getToolClasses(); bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) @@ -352,4 +352,4 @@ void ClusterLabelDisplay::addLabel(std::string label, std::vector face } // End namespace rviz_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelDisplay, rviz::Display) +PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/ClusterLabelPanel.cpp b/rviz_map_plugin/src/ClusterLabelPanel.cpp index 4ff4728..e03a0db 100644 --- a/rviz_map_plugin/src/ClusterLabelPanel.cpp +++ b/rviz_map_plugin/src/ClusterLabelPanel.cpp @@ -60,7 +60,7 @@ namespace rviz_map_plugin { -ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz::Panel(parent) +ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz_common::Panel(parent) { QHBoxLayout* clusterNameLayout = new QHBoxLayout(); clusterNameLayout->addWidget(new QLabel("Cluster Name:")); @@ -87,7 +87,7 @@ ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz::Panel(parent) void ClusterLabelPanel::onInitialize() { // Check if the cluster label tool is already opened - rviz::ToolManager* toolManager = vis_manager_->getToolManager(); + rviz_common::ToolManager* toolManager = vis_manager_->getToolManager(); QStringList toolClasses = toolManager->getToolClasses(); bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) @@ -132,15 +132,15 @@ void ClusterLabelPanel::resetFaces() m_tool->resetFaces(); } -void ClusterLabelPanel::save(rviz::Config config) const +void ClusterLabelPanel::save(rviz_common::Config config) const { - rviz::Panel::save(config); + rviz_common::Panel::save(config); config.mapSetValue("ClusterName", m_clusterName); } -void ClusterLabelPanel::load(const rviz::Config& config) +void ClusterLabelPanel::load(const rviz_common::Config& config) { - rviz::Panel::load(config); + rviz_common::Panel::load(config); QString clusterName; if (config.mapGetString("ClusterName", &clusterName)) ; @@ -153,4 +153,4 @@ void ClusterLabelPanel::load(const rviz::Config& config) } // End namespace rviz_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelPanel, rviz::Panel) +PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelPanel, rviz_common::Panel) diff --git a/rviz_map_plugin/src/ClusterLabelTool.cpp b/rviz_map_plugin/src/ClusterLabelTool.cpp index c954790..1eae958 100644 --- a/rviz_map_plugin/src/ClusterLabelTool.cpp +++ b/rviz_map_plugin/src/ClusterLabelTool.cpp @@ -62,7 +62,7 @@ #include #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelTool, rviz::Tool) +PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelTool, rviz_common::Tool) using std::ifstream; using std::stringstream; @@ -345,7 +345,7 @@ void ClusterLabelTool::updateSelectionBox() m_selectionBox->end(); } -void ClusterLabelTool::selectionBoxStart(rviz::ViewportMouseEvent& event) +void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) { m_selectionStart.x = (float)event.x / event.viewport->getActualWidth(); m_selectionStart.y = (float)event.y / event.viewport->getActualHeight(); @@ -354,14 +354,14 @@ void ClusterLabelTool::selectionBoxStart(rviz::ViewportMouseEvent& event) m_selectionBox->setVisible(true); } -void ClusterLabelTool::selectionBoxMove(rviz::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(); updateSelectionBox(); } -void ClusterLabelTool::selectMultipleFaces(rviz::ViewportMouseEvent& event, bool selectMode) +void ClusterLabelTool::selectMultipleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode) { m_selectionBox->setVisible(false); @@ -454,7 +454,7 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume } } -void ClusterLabelTool::selectSingleFace(rviz::ViewportMouseEvent& event, bool selectMode) +void ClusterLabelTool::selectSingleFace(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()); @@ -522,7 +522,7 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) } } -void ClusterLabelTool::selectSphereFaces(rviz::ViewportMouseEvent& event, bool selectMode) +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()); @@ -648,7 +648,7 @@ void ClusterLabelTool::publishLabel(std::string label) } // Handling mouse event and mark the clicked faces -int ClusterLabelTool::processMouseEvent(rviz::ViewportMouseEvent& event) +int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { if (event.leftDown() && event.control()) { diff --git a/rviz_map_plugin/src/ClusterLabelVisual.cpp b/rviz_map_plugin/src/ClusterLabelVisual.cpp index ff7c611..e636aaf 100644 --- a/rviz_map_plugin/src/ClusterLabelVisual.cpp +++ b/rviz_map_plugin/src/ClusterLabelVisual.cpp @@ -47,7 +47,7 @@ #include -#include +#include #include #include @@ -59,12 +59,12 @@ namespace rviz_map_plugin { -ClusterLabelVisual::ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId) +ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId) : m_displayContext(context), m_labelId(labelId) { } -ClusterLabelVisual::ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId, +ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId, std::shared_ptr geometry) : m_displayContext(context), m_labelId(labelId), m_geometry(geometry) { diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index bbd3f2d..a0298d4 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -70,7 +70,7 @@ namespace rviz_map_plugin { MapDisplay::MapDisplay() { - m_mapFilePath = new rviz::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, + m_mapFilePath = new rviz_common::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, SLOT(updateMap())); } @@ -93,14 +93,14 @@ std::shared_ptr MapDisplay::getGeometry() // ===================================================================================================================== // Callbacks -rviz::Display* MapDisplay::createDisplay(const QString& class_id) +rviz_common::Display* MapDisplay::createDisplay(const QString& class_id) { - rviz::DisplayFactory* factory = context_->getDisplayFactory(); + rviz_common::DisplayFactory* factory = context_->getDisplayFactory(); QString error; - rviz::Display* disp = factory->make(class_id, &error); + rviz_common::Display* disp = factory->make(class_id, &error); if (!disp) { - return new rviz::FailedDisplay(class_id, error); + return new rviz_common::FailedDisplay(class_id, error); } return disp; } @@ -150,12 +150,12 @@ void MapDisplay::onDisable() // ===================================================================================================================== // Callbacks triggered from UI events (mostly) -void MapDisplay::load(const rviz::Config& config) +void MapDisplay::load(const rviz_common::Config& config) { std::string name = this->getName().toStdString(); std::cout << name << ": LOAD CONFIG..." << std::endl; - rviz::Config config2 = config; + rviz_common::Config config2 = config; { // Override with ros params std::stringstream ss; @@ -170,7 +170,7 @@ void MapDisplay::load(const rviz::Config& config) } } - rviz::Display::load(config2); + rviz_common::Display::load(config2); std::cout << name << ": LOAD CONFIG done." << std::endl; } @@ -206,7 +206,7 @@ void MapDisplay::updateMap() m_clusterLabelDisplay->setData(m_geometry, m_clusterList); // All good - setStatus(rviz::StatusProperty::Ok, "Map", ""); + setStatus(rviz_common::StatusProperty::Ok, "Map", ""); m_map_file_loaded = m_mapFilePath->getFilename(); } @@ -245,13 +245,13 @@ bool MapDisplay::loadData() if (mapFile.empty()) { ROS_WARN_STREAM("Map Display: No map file path specified!"); - setStatus(rviz::StatusProperty::Warn, "Map", "No map file path specified!"); + setStatus(rviz_common::StatusProperty::Warn, "Map", "No map file path specified!"); return false; } if (!boost::filesystem::exists(mapFile)) { ROS_WARN_STREAM("Map Display: Specified map file does not exist!"); - setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file does not exist!"); + setStatus(rviz_common::StatusProperty::Warn, "Map", "Specified map file does not exist!"); return false; } @@ -439,11 +439,11 @@ bool MapDisplay::loadData() catch (...) { ROS_ERROR_STREAM("An unexpected error occurred while using Pluto Map IO"); - setStatus(rviz::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); + setStatus(rviz_common::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); return false; } - setStatus(rviz::StatusProperty::Ok, "IO", ""); + setStatus(rviz_common::StatusProperty::Ok, "IO", ""); ROS_INFO("Map Display: Successfully loaded map."); @@ -468,7 +468,7 @@ void MapDisplay::saveLabel(Cluster cluster) if (results.size() != 2) { ROS_ERROR_STREAM("Map Display: Illegal label name '" << label << "'"); - setStatus(rviz::StatusProperty::Error, "Label", "Illegal label name!"); + setStatus(rviz_common::StatusProperty::Error, "Label", "Illegal label name!"); return; } @@ -481,7 +481,7 @@ void MapDisplay::saveLabel(Cluster cluster) // Add to cluster list m_clusterList.push_back(Cluster(label, faces)); - setStatus(rviz::StatusProperty::Ok, "Label", "Successfully saved label"); + setStatus(rviz_common::StatusProperty::Ok, "Label", "Successfully saved label"); ROS_INFO_STREAM("Map Display: Successfully added label to map."); // update the map to show the new label @@ -489,11 +489,11 @@ void MapDisplay::saveLabel(Cluster cluster) } catch (...) { - setStatus(rviz::StatusProperty::Error, "Label", "Error while saving label"); + setStatus(rviz_common::StatusProperty::Error, "Label", "Error while saving label"); } } } // End namespace rviz_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MapDisplay, rviz::Display) +PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MapDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index 00872d0..3dacc2a 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -53,36 +53,41 @@ #include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + namespace rviz_map_plugin { -MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) +MeshDisplay::MeshDisplay() +: rviz_common::Display() +, m_ignoreMsgs(false) { // mesh topic - m_meshTopic = new rviz::RosTopicProperty( + m_meshTopic = new rviz_common::RosTopicProperty( "Geometry Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized - m_bufferSize = new rviz::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); + m_bufferSize = new rviz_common::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); m_bufferSize->setMin(1); // Display Type { - m_displayType = new rviz::EnumProperty("Display Type", "Fixed Color", "Select Display Type for Mesh", this, + m_displayType = new rviz_common::EnumProperty("Display Type", "Fixed Color", "Select Display Type for Mesh", this, SLOT(updateMesh()), this); m_displayType->addOption("Fixed Color", 0); m_displayType->addOption("Vertex Color", 1); @@ -93,11 +98,11 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Fixed Color { // face color properties - m_facesColor = new rviz::ColorProperty("Faces Color", QColor(0, 255, 0), "The color of the faces.", m_displayType, + m_facesColor = new rviz_common::ColorProperty("Faces Color", QColor(0, 255, 0), "The color of the faces.", m_displayType, SLOT(updateMesh()), this); // face alpha properties - m_facesAlpha = new rviz::FloatProperty("Faces Alpha", 1.0, "The alpha-value of the faces", m_displayType, + m_facesAlpha = new rviz_common::FloatProperty("Faces Alpha", 1.0, "The alpha-value of the faces", m_displayType, SLOT(updateMesh()), this); m_facesAlpha->setMin(0); m_facesAlpha->setMax(1); @@ -105,12 +110,12 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Vertex Color { - m_vertexColorsTopic = new rviz::RosTopicProperty( + m_vertexColorsTopic = new rviz_common::RosTopicProperty( "Vertex Colors Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Vertex color topic to subscribe to.", m_displayType, SLOT(updateVertexColorsTopic()), this); - m_vertexColorServiceName = new rviz::StringProperty("Vertex Color Service Name", "get_vertex_colors", + m_vertexColorServiceName = new rviz_common::StringProperty("Vertex Color Service Name", "get_vertex_colors", "Name of the Vertex Color Service to request Vertex Colors " "from.", m_displayType, SLOT(updateVertexColorService()), this); @@ -118,48 +123,48 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Textures { - m_showTexturedFacesOnly = new rviz::BoolProperty("Show textured faces only", false, "Show textured faces only", + m_showTexturedFacesOnly = new rviz_common::BoolProperty("Show textured faces only", false, "Show textured faces only", m_displayType, SLOT(updateMesh()), this); - m_materialServiceName = new rviz::StringProperty("Material Service Name", "get_materials", + m_materialServiceName = new rviz_common::StringProperty("Material Service Name", "get_materials", "Name of the Matrial Service to request Materials from.", m_displayType, SLOT(updateMaterialAndTextureServices()), this); - m_textureServiceName = new rviz::StringProperty("Texture Service Name", "get_texture", + m_textureServiceName = new rviz_common::StringProperty("Texture Service Name", "get_texture", "Name of the Texture Service to request Textures from.", m_displayType, SLOT(updateMaterialAndTextureServices()), this); } // Vertex Costs { - m_costColorType = new rviz::EnumProperty("Color Scale", "Rainbow", + m_costColorType = new rviz_common::EnumProperty("Color Scale", "Rainbow", "Select color scale for vertex costs. Mesh will update when new data " "arrives.", m_displayType, SLOT(updateVertexCosts()), this); m_costColorType->addOption("Rainbow", 0); m_costColorType->addOption("Red Green", 1); - m_vertexCostsTopic = new rviz::RosTopicProperty( + m_vertexCostsTopic = new rviz_common::RosTopicProperty( "Vertex Costs Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Vertex cost topic to subscribe to.", m_displayType, SLOT(updateVertexCostsTopic()), this); - m_selectVertexCostMap = new rviz::EnumProperty("Vertex Costs Type", "-- None --", + m_selectVertexCostMap = new rviz_common::EnumProperty("Vertex Costs Type", "-- None --", "Select the type of vertex cost map to be displayed. New types " "will appear here when a new message arrives.", m_displayType, SLOT(updateVertexCosts()), this); m_selectVertexCostMap->addOption("-- None --", 0); - m_costUseCustomLimits = new rviz::BoolProperty("Use Custom limits", false, "Use custom vertex cost limits", + m_costUseCustomLimits = new rviz_common::BoolProperty("Use Custom limits", false, "Use custom vertex cost limits", m_displayType, SLOT(updateVertexCosts()), this); // custom cost limits { - m_costLowerLimit = new rviz::FloatProperty("Vertex Costs Lower Limit", 0.0, "Vertex costs lower limit", + m_costLowerLimit = new rviz_common::FloatProperty("Vertex Costs Lower Limit", 0.0, "Vertex costs lower limit", m_costUseCustomLimits, SLOT(updateVertexCosts()), this); m_costLowerLimit->hide(); - m_costUpperLimit = new rviz::FloatProperty("Vertex Costs Upper Limit", 1.0, "Vertex costs upper limit", + m_costUpperLimit = new rviz_common::FloatProperty("Vertex Costs Upper Limit", 1.0, "Vertex costs upper limit", m_costUseCustomLimits, SLOT(updateVertexCosts()), this); m_costUpperLimit->hide(); } @@ -169,13 +174,13 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Wireframe { m_showWireframe = - new rviz::BoolProperty("Show Wireframe", true, "Show Wireframe", this, SLOT(updateWireframe()), this); + new rviz_common::BoolProperty("Show Wireframe", true, "Show Wireframe", this, SLOT(updateWireframe()), this); // wireframe color property - m_wireframeColor = new rviz::ColorProperty("Wireframe Color", QColor(0, 0, 0), "The color of the wireframe.", + m_wireframeColor = new rviz_common::ColorProperty("Wireframe Color", QColor(0, 0, 0), "The color of the wireframe.", m_showWireframe, SLOT(updateWireframe()), this); // wireframe alpha property - m_wireframeAlpha = new rviz::FloatProperty("Wireframe Alpha", 1.0, "The alpha-value of the wireframe", + m_wireframeAlpha = new rviz_common::FloatProperty("Wireframe Alpha", 1.0, "The alpha-value of the wireframe", m_showWireframe, SLOT(updateWireframe()), this); m_wireframeAlpha->setMin(0); m_wireframeAlpha->setMax(1); @@ -183,15 +188,15 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Normals { - m_showNormals = new rviz::BoolProperty("Show Normals", true, "Show Normals", this, SLOT(updateNormals()), this); + m_showNormals = new rviz_common::BoolProperty("Show Normals", true, "Show Normals", this, SLOT(updateNormals()), this); - m_normalsColor = new rviz::ColorProperty("Normals Color", QColor(255, 0, 255), "The color of the normals.", + m_normalsColor = new rviz_common::ColorProperty("Normals Color", QColor(255, 0, 255), "The color of the normals.", m_showNormals, SLOT(updateNormalsColor()), this); - m_normalsAlpha = new rviz::FloatProperty("Normals Alpha", 1.0, "The alpha-value of the normals", m_showNormals, + m_normalsAlpha = new rviz_common::FloatProperty("Normals Alpha", 1.0, "The alpha-value of the normals", m_showNormals, SLOT(updateNormalsColor()), this); m_normalsAlpha->setMin(0); m_normalsAlpha->setMax(1); - m_scalingFactor = new rviz::FloatProperty("Normals Scaling Factor", 0.1, "Scaling factor of the normals", + m_scalingFactor = new rviz_common::FloatProperty("Normals Scaling Factor", 0.1, "Scaling factor of the normals", m_showNormals, SLOT(updateNormalsSize()), this); } } @@ -203,20 +208,20 @@ MeshDisplay::~MeshDisplay() void MeshDisplay::onInitialize() { m_tfMeshFilter = new tf2_ros::MessageFilter( - *rviz::Display::context_->getTF2BufferPtr(), rviz::Display::fixed_frame_.toStdString(), 2, - rviz::Display::update_nh_); + *rviz_common::Display::context_->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 2, + rviz_common::Display::update_nh_); m_tfMeshFilter->connectInput(m_meshSubscriber); context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfMeshFilter, this); m_tfVertexColorsFilter = new tf2_ros::MessageFilter( - *rviz::Display::context_->getTF2BufferPtr(), rviz::Display::fixed_frame_.toStdString(), 10, - rviz::Display::update_nh_); + *rviz_common::Display::context_->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, + rviz_common::Display::update_nh_); m_tfVertexColorsFilter->connectInput(m_vertexColorsSubscriber); context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexColorsFilter, this); m_tfVertexCostsFilter = new tf2_ros::MessageFilter( - *rviz::Display::context_->getTF2BufferPtr(), rviz::Display::fixed_frame_.toStdString(), 10, - rviz::Display::update_nh_); + *rviz_common::Display::context_->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, + rviz_common::Display::update_nh_); m_tfVertexCostsFilter->connectInput(m_vertexCostsSubscriber); context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexCostsFilter, this); @@ -267,11 +272,11 @@ void MeshDisplay::subscribe() m_meshSubscriber.subscribe(update_nh_, m_meshTopic->getTopicStd(), 1); m_vertexColorsSubscriber.subscribe(update_nh_, m_vertexColorsTopic->getTopicStd(), 1); m_vertexCostsSubscriber.subscribe(update_nh_, m_vertexCostsTopic->getTopicStd(), 4); - setStatus(rviz::StatusProperty::Ok, "Topic", "OK"); + setStatus(rviz_common::StatusProperty::Ok, "Topic", "OK"); } catch (ros::Exception& e) { - setStatus(rviz::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); + setStatus(rviz_common::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); } // Nothing @@ -344,7 +349,7 @@ void MeshDisplay::setGeometry(shared_ptr geometry) updateNormals(); updateWireframe(); } - setStatus(rviz::StatusProperty::Ok, "Display", ""); + setStatus(rviz_common::StatusProperty::Ok, "Display", ""); } void MeshDisplay::setVertexColors(vector& vertexColors) @@ -631,7 +636,7 @@ void MeshDisplay::updateMaterialAndTextureServices() if (!ros::names::validate(m_materialServiceName->getStdString(), error) || !ros::names::validate(m_textureServiceName->getStdString(), error)) { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); + setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } @@ -644,16 +649,16 @@ void MeshDisplay::updateMaterialAndTextureServices() requestMaterials(m_lastUuid); if (m_textureClient.exists()) { - setStatus(rviz::StatusProperty::Ok, "Services", "Material and Texture Service OK"); + setStatus(rviz_common::StatusProperty::Ok, "Services", "Material and Texture Service OK"); } else { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); + setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); } } else { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); + setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); } } @@ -668,7 +673,7 @@ void MeshDisplay::updateVertexColorService() std::string error; if (!ros::names::validate(m_vertexColorServiceName->getStdString(), error)) { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); + setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } @@ -677,12 +682,12 @@ void MeshDisplay::updateVertexColorService() m_vertexColorClient = n.serviceClient(m_vertexColorServiceName->getStdString()); if (m_vertexColorClient.exists()) { - setStatus(rviz::StatusProperty::Ok, "Services", "Vertex Color Service OK"); + setStatus(rviz_common::StatusProperty::Ok, "Services", "Vertex Color Service OK"); requestVertexColors(m_lastUuid); } else { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist.")); + setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist.")); } } @@ -747,7 +752,7 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& orientation)) { ROS_ERROR("Error transforming from frame '%s' to frame '%s'", meshMsg->header.frame_id.c_str(), - qPrintable(rviz::Display::fixed_frame_)); + qPrintable(rviz_common::Display::fixed_frame_)); return; } @@ -798,7 +803,7 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& void MeshDisplay::incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg) { m_messagesReceived++; - setStatus(rviz::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); + setStatus(rviz_common::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); processMessage(meshMsg); } @@ -991,5 +996,5 @@ std::shared_ptr MeshDisplay::addNewVisual() } // End namespace rviz_map_plugin -#include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MeshDisplay, rviz::Display) +#include +PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MeshDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/MeshGoalTool.cpp b/rviz_map_plugin/src/MeshGoalTool.cpp index 8d7990c..75f65ab 100644 --- a/rviz_map_plugin/src/MeshGoalTool.cpp +++ b/rviz_map_plugin/src/MeshGoalTool.cpp @@ -46,17 +46,17 @@ #include "MeshGoalTool.hpp" #include -PLUGINLIB_EXPORT_CLASS( rviz_map_plugin::MeshGoalTool, rviz::Tool ) +PLUGINLIB_EXPORT_CLASS( rviz_map_plugin::MeshGoalTool, rviz_common::Tool ) namespace rviz_map_plugin{ MeshGoalTool::MeshGoalTool() { shortcut_key_ = 'm'; - topic_property_ = new rviz::StringProperty( "Topic", "goal", + topic_property_ = new rviz_common::StringProperty( "Topic", "goal", "The topic on which to publish the mesh navigation goals.", getPropertyContainer(), SLOT(updateTopic()), this); - switch_bottom_top_ = new rviz::BoolProperty("Switch Bottom/Top", + switch_bottom_top_ = new rviz_common::BoolProperty("Switch Bottom/Top", false, "Enable to stwich the bottom and top.", getPropertyContainer()); diff --git a/rviz_map_plugin/src/MeshPoseTool.cpp b/rviz_map_plugin/src/MeshPoseTool.cpp index 54938a0..26f1bbd 100644 --- a/rviz_map_plugin/src/MeshPoseTool.cpp +++ b/rviz_map_plugin/src/MeshPoseTool.cpp @@ -53,13 +53,13 @@ #include #include #include -#include +#include #include "MeshPoseTool.hpp" namespace rviz_map_plugin { -MeshPoseTool::MeshPoseTool() : rviz::Tool(), arrow_(NULL) +MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { } @@ -70,7 +70,7 @@ MeshPoseTool::~MeshPoseTool() void MeshPoseTool::onInitialize() { - arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_ = new rviz_common::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); arrow_->getSceneNode()->setVisible(false); } @@ -86,7 +86,7 @@ void MeshPoseTool::deactivate() arrow_->getSceneNode()->setVisible(false); } -int MeshPoseTool::processMouseEvent(rviz::ViewportMouseEvent& event) +int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { int flags = 0; @@ -110,7 +110,7 @@ int MeshPoseTool::processMouseEvent(rviz::ViewportMouseEvent& event) { Ogre::Vector3 cur_pos; Ogre::Plane plane(ori_, pos_); - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) + if (rviz_common::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) { // right hand coordinate system // x to the right, y to the top, and -z into the scene @@ -136,7 +136,7 @@ int MeshPoseTool::processMouseEvent(rviz::ViewportMouseEvent& event) { Ogre::Vector3 cur_pos; Ogre::Plane plane(ori_, pos_); - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) + if (rviz_common::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) { // arrow foreward negative z Ogre::Vector3 z_axis = -(cur_pos - pos_); @@ -157,7 +157,7 @@ int MeshPoseTool::processMouseEvent(rviz::ViewportMouseEvent& event) return flags; } -bool MeshPoseTool::selectTriangle(rviz::ViewportMouseEvent& event, Ogre::Vector3& position, +bool MeshPoseTool::selectTriangle(rviz_common::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& triangle_normal) { Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( diff --git a/rviz_map_plugin/src/MeshVisual.cpp b/rviz_map_plugin/src/MeshVisual.cpp index 4dd59b9..1e2102e 100644 --- a/rviz_map_plugin/src/MeshVisual.cpp +++ b/rviz_map_plugin/src/MeshVisual.cpp @@ -93,7 +93,7 @@ Ogre::ColourValue getRainbowColor1(float value) return Ogre::ColourValue(r, g, b, 1.0f); } -MeshVisual::MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID) +MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID) : m_displayContext(context) , m_prefix(displayID) , m_postfix(meshID) From 5dafd894b6e5163f05a3e7aad494766c01bf9fec Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 12:41:27 +0100 Subject: [PATCH 15/40] added comment --- mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h index e14b173..5e6bffc 100644 --- a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h +++ b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h @@ -15,7 +15,7 @@ #include "mesh_msgs/srv/get_geometry.hpp" #include "mesh_msgs/srv/get_materials.hpp" #include "mesh_msgs/srv/get_texture.hpp" -#include "mesh_msgs/srv/get_uui_ds.hpp" // :D +#include "mesh_msgs/srv/get_uui_ds.hpp" // :D GetUUIDs #include "mesh_msgs/srv/get_vertex_colors.hpp" #include "mesh_msgs/srv/get_vertex_costs.hpp" #include "mesh_msgs/srv/get_vertex_cost_layers.hpp" From b0d59ee7363ca51c3d9e0bbc45a5a3917ec410a6 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 12:41:46 +0100 Subject: [PATCH 16/40] more fixes --- rviz_map_plugin/CMakeLists.txt | 2 + rviz_map_plugin/COLCON_IGNORE | 0 .../include/ClusterLabelDisplay.hpp | 14 +-- rviz_map_plugin/include/MapDisplay.hpp | 2 +- rviz_map_plugin/include/MeshDisplay.hpp | 32 +++---- rviz_map_plugin/package.xml | 1 + rviz_map_plugin/src/ClusterLabelDisplay.cpp | 26 +++--- rviz_map_plugin/src/ClusterLabelPanel.cpp | 4 +- rviz_map_plugin/src/ClusterLabelTool.cpp | 62 ++++++------- rviz_map_plugin/src/ClusterLabelVisual.cpp | 10 +-- rviz_map_plugin/src/MapDisplay.cpp | 36 ++++---- rviz_map_plugin/src/MeshDisplay.cpp | 88 ++++++++++--------- rviz_map_plugin/src/MeshPoseTool.cpp | 2 +- rviz_map_plugin/src/MeshVisual.cpp | 48 +++++----- 14 files changed, 169 insertions(+), 158 deletions(-) delete mode 100644 rviz_map_plugin/COLCON_IGNORE diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index 0d02242..f6b15ac 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(hdf5_map_io REQUIRED) find_package(mesh_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) +find_package(message_filters REQUIRED) find_package(Boost REQUIRED COMPONENTS system) @@ -95,6 +96,7 @@ ament_target_dependencies(${PROJECT_NAME} rviz_rendering tf2_ros pluginlib + message_filters ) diff --git a/rviz_map_plugin/COLCON_IGNORE b/rviz_map_plugin/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/rviz_map_plugin/include/ClusterLabelDisplay.hpp b/rviz_map_plugin/include/ClusterLabelDisplay.hpp index 040cde0..2778956 100644 --- a/rviz_map_plugin/include/ClusterLabelDisplay.hpp +++ b/rviz_map_plugin/include/ClusterLabelDisplay.hpp @@ -266,28 +266,30 @@ private Q_SLOTS: ClusterLabelTool* m_tool; /// Property for the current active visual - rviz_common::EnumProperty* m_activeVisualProperty; + rviz_common::properties::EnumProperty* m_activeVisualProperty; /// Property to set transparency - rviz_common::FloatProperty* m_alphaProperty; + rviz_common::properties::FloatProperty* m_alphaProperty; /// Property for selecting colors (menu) - rviz_common::Property* m_colorsProperty; + rviz_common::properties::Property* m_colorsProperty; /// Properties for selecting colors (menu-items) - std::vector m_colorProperties; + std::vector m_colorProperties; /// Property to set the brushsize of the sphere brush of the label tool from this package - rviz_common::FloatProperty* m_sphereSizeProperty; + rviz_common::properties::FloatProperty* m_sphereSizeProperty; /// Property to hide or show a phantom visual - rviz_common::BoolProperty* m_phantomVisualProperty; + rviz_common::properties::BoolProperty* m_phantomVisualProperty; /// Index for the visuals int m_labelToolVisualIndex = 0; /// A variable that will be set to true, once the initial data has arrived bool has_data = false; + + }; } // end namespace rviz_map_plugin diff --git a/rviz_map_plugin/include/MapDisplay.hpp b/rviz_map_plugin/include/MapDisplay.hpp index 2df81c7..c06deb6 100644 --- a/rviz_map_plugin/include/MapDisplay.hpp +++ b/rviz_map_plugin/include/MapDisplay.hpp @@ -96,7 +96,7 @@ #include #ifndef Q_MOC_RUN -#include +#include #include #include diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_map_plugin/include/MeshDisplay.hpp index c745814..4a13d34 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_map_plugin/include/MeshDisplay.hpp @@ -81,9 +81,9 @@ #include #ifndef Q_MOC_RUN -// #include -// #include -// #include +#include +#include +#include #include @@ -298,19 +298,19 @@ private Q_SLOTS: * @brief Sets data for trianglemesh_visual and updates the mesh. * @param meshMsg Message containing geometry information */ - void processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg); + void processMessage(const mesh_msgs::msg::MeshGeometryStamped::ConstPtr& meshMsg); /** * @brief Handler for incoming geometry messages. Validate data and update mesh * @param meshMsg The geometry */ - void incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg); + void incomingGeometry(const mesh_msgs::msg::MeshGeometryStamped::ConstPtr& meshMsg); /** * @brief Handler for incoming vertex color messages. Validate data and update mesh * @param colorsStamped The vertex colors */ - void incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr& colorsStamped); + void incomingVertexColors(const mesh_msgs::msg::MeshVertexColorsStamped::ConstPtr& colorsStamped); /** * @brief Handler for incoming vertex cost messages. Validate data and update mesh @@ -351,6 +351,8 @@ private Q_SLOTS: /// if set to true, ignore incoming messages and do not use services to request materials bool m_ignoreMsgs; + rclcpp::Node m_node; + /// Client to request the vertex colors ros::ServiceClient m_vertexColorClient; @@ -367,31 +369,31 @@ private Q_SLOTS: ros::ServiceClient m_geometryClient; /// Subscriber for meshMsg - message_filters::Subscriber m_meshSubscriber; + message_filters::Subscriber m_meshSubscriber; /// Subscriber for vertex colors - message_filters::Subscriber m_vertexColorsSubscriber; + message_filters::Subscriber m_vertexColorsSubscriber; /// Subscriber for vertex costs - message_filters::Subscriber m_vertexCostsSubscriber; + message_filters::Subscriber m_vertexCostsSubscriber; /// Messagefilter for meshMsg - tf2_ros::MessageFilter* m_tfMeshFilter; + tf2_ros::MessageFilter* m_tfMeshFilter; /// Messagefilter for vertex colors - tf2_ros::MessageFilter* m_tfVertexColorsFilter; + tf2_ros::MessageFilter* m_tfVertexColorsFilter; /// Messagefilter for vertex costs - tf2_ros::MessageFilter* m_tfVertexCostsFilter; + tf2_ros::MessageFilter* m_tfVertexCostsFilter; /// Synchronizer for meshMsgs - message_filters::Cache* m_meshSynchronizer; + message_filters::Cache* m_meshSynchronizer; /// Synchronizer for vertex colors - message_filters::Cache* m_colorsSynchronizer; + message_filters::Cache* m_colorsSynchronizer; /// Synchronizer for vertex costs - message_filters::Cache* m_costsSynchronizer; + message_filters::Cache* m_costsSynchronizer; /// Counter for the received messages uint32_t m_messagesReceived; diff --git a/rviz_map_plugin/package.xml b/rviz_map_plugin/package.xml index a26e626..95e801b 100644 --- a/rviz_map_plugin/package.xml +++ b/rviz_map_plugin/package.xml @@ -27,6 +27,7 @@ hdf5_map_io tf2_ros pluginlib + message_filters ocl-icd-opencl-dev opencl-headers libqt5-core diff --git a/rviz_map_plugin/src/ClusterLabelDisplay.cpp b/rviz_map_plugin/src/ClusterLabelDisplay.cpp index f3c3d0a..de7fc95 100644 --- a/rviz_map_plugin/src/ClusterLabelDisplay.cpp +++ b/rviz_map_plugin/src/ClusterLabelDisplay.cpp @@ -57,6 +57,8 @@ #include #include +#include "rclcpp/rclcpp.hpp" + namespace rviz_map_plugin { Ogre::ColourValue getRainbowColor(float value) @@ -92,9 +94,9 @@ Ogre::ColourValue getRainbowColor(float value) ClusterLabelDisplay::ClusterLabelDisplay() { m_activeVisualProperty = - new rviz_common::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", + new rviz_common::properties::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", this, SLOT(changeVisual()), this); - m_alphaProperty = new rviz_common::FloatProperty("Transparency", 1.0f, + m_alphaProperty = new rviz_common::properties::FloatProperty("Transparency", 1.0f, "Transparency of the Labeled Cluster Visualization. 0.0 is fully " "transparent, 1.0 fully opaque", this, SLOT(updateColors()), this); @@ -105,7 +107,7 @@ ClusterLabelDisplay::ClusterLabelDisplay() m_colorsProperty->setReadOnly(true); m_sphereSizeProperty = new rviz_common::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); - m_phantomVisualProperty = new rviz_common::BoolProperty("Show Phantom", false, + m_phantomVisualProperty = new rviz_common::properties::BoolProperty("Show Phantom", false, "Show a transparent silhouette of the whole mesh to help with " "labeling", this, SLOT(updatePhantomVisual()), this); @@ -124,7 +126,7 @@ std::shared_ptr ClusterLabelDisplay::getGeometry() { if (!m_geometry) { - ROS_ERROR("Label Display: Geometry requested, but none available!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Geometry requested, but none available!"); } return m_geometry; } @@ -133,7 +135,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector { if (has_data) { - ROS_WARN("Label Display: already has data, but setData() was called again!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: already has data, but setData() was called again!"); } // Copy data @@ -142,7 +144,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector m_clusterList.insert(m_clusterList.begin(), Cluster("__NEW__", vector())); // Set flag - ROS_INFO("Label Display: received data"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: received data"); has_data = true; // Draw visuals @@ -180,11 +182,11 @@ void ClusterLabelDisplay::changeVisual() { if (m_activeVisualProperty->getStdString().empty()) { - ROS_ERROR("Label Display: Should change visual but no visual selected!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Should change visual but no visual selected!"); return; } - ROS_INFO_STREAM("Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); m_activeVisualId = m_activeVisualProperty->getOptionInt(); @@ -194,11 +196,11 @@ void ClusterLabelDisplay::changeVisual() void ClusterLabelDisplay::updateMap() { - ROS_INFO("Label Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Update"); if (!has_data) { - ROS_WARN("Label Display: No data available! Can't show map"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: No data available! Can't show map"); return; } @@ -293,7 +295,7 @@ void ClusterLabelDisplay::createVisualsFromClusterList() ss << "ClusterLabelVisual_" << i; auto visual = std::make_shared(context_, ss.str(), m_geometry); - ROS_DEBUG_STREAM("Label Display: Create visual for label '" << m_clusterList[i].name << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Create visual for label '" << m_clusterList[i].name << "'"); visual->setFacesInCluster(m_clusterList[i].faces); visual->setColor(getRainbowColor((++colorIndex / m_clusterList.size())), m_alphaProperty->getFloat()); m_visuals.push_back(visual); @@ -344,7 +346,7 @@ void ClusterLabelDisplay::notifyLabelTool() void ClusterLabelDisplay::addLabel(std::string label, std::vector faces) { - ROS_INFO_STREAM("Cluster Label Display: add label '" << label << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Cluster Label Display: add label '" << label << "'"); Q_EMIT signalAddLabel(Cluster(label, faces)); } diff --git a/rviz_map_plugin/src/ClusterLabelPanel.cpp b/rviz_map_plugin/src/ClusterLabelPanel.cpp index e03a0db..4c5aabb 100644 --- a/rviz_map_plugin/src/ClusterLabelPanel.cpp +++ b/rviz_map_plugin/src/ClusterLabelPanel.cpp @@ -122,13 +122,13 @@ void ClusterLabelPanel::updateClusterName() void ClusterLabelPanel::publish() { - ROS_INFO("Label Panel: Publish"); + RCLCPP_INFO("Label Panel: Publish"); m_tool->publishLabel(m_clusterName.toStdString()); } void ClusterLabelPanel::resetFaces() { - ROS_INFO("Label panel: Reset"); + RCLCPP_INFO("Label panel: Reset"); m_tool->resetFaces(); } diff --git a/rviz_map_plugin/src/ClusterLabelTool.cpp b/rviz_map_plugin/src/ClusterLabelTool.cpp index 1eae958..15456a2 100644 --- a/rviz_map_plugin/src/ClusterLabelTool.cpp +++ b/rviz_map_plugin/src/ClusterLabelTool.cpp @@ -91,7 +91,7 @@ ClusterLabelTool::~ClusterLabelTool() // context_ are set. It should be called only once per instantiation. void ClusterLabelTool::onInitialize() { - ROS_DEBUG("ClusterLabelTool: Call Init"); + RCLCPP_DEBUG("ClusterLabelTool: Call Init"); m_sceneNode = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode(); m_selectionBox = context_->getSceneManager()->createManualObject("ClusterLabelTool_SelectionBox"); @@ -115,28 +115,28 @@ void ClusterLabelTool::onInitialize() try { // Initialize OpenCL - ROS_DEBUG("Get platforms"); + RCLCPP_DEBUG("Get platforms"); vector platforms; cl::Platform::get(&platforms); for (auto const& platform : platforms) { - ROS_DEBUG("Found platform: %s", platform.getInfo().c_str()); - ROS_DEBUG("platform version: %s", platform.getInfo().c_str()); + RCLCPP_DEBUG("Found platform: %s", platform.getInfo().c_str()); + RCLCPP_DEBUG("platform version: %s", platform.getInfo().c_str()); } - ROS_DEBUG(" "); + RCLCPP_DEBUG(" "); vector consideredDevices; for (auto const& platform : platforms) { - ROS_DEBUG("Get devices of %s: ", platform.getInfo().c_str()); + RCLCPP_DEBUG("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) { - ROS_DEBUG("Found device: %s", device.getInfo().c_str()); - ROS_DEBUG("Device work units: %d", device.getInfo()); - ROS_DEBUG("Device work group size: %lu", device.getInfo()); + RCLCPP_DEBUG("Found device: %s", device.getInfo().c_str()); + RCLCPP_DEBUG("Device work units: %d", device.getInfo()); + RCLCPP_DEBUG("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 @@ -148,7 +148,7 @@ void ClusterLabelTool::onInitialize() // use bitwise AND to extract the number in the lower 16 bits cl_uint minorVersion = version & 0x0000FFFF; - ROS_INFO("Found a device with OpenCL version: %u.%u", majorVersion, minorVersion); + RCLCPP_INFO("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) @@ -158,7 +158,7 @@ void ClusterLabelTool::onInitialize() } } } - ROS_DEBUG(" "); + RCLCPP_DEBUG(" "); cl::Platform platform; // Preferably choose the first compatible device of type GPU @@ -183,22 +183,22 @@ void ClusterLabelTool::onInitialize() if (!deviceFound) { // Panic if no compatible device was found - ROS_ERROR("No device with compatible OpenCL version found (minimum 2.0)"); + RCLCPP_ERROR("No device with compatible OpenCL version found (minimum 2.0)"); ros::requestShutdown(); } cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); - ROS_INFO("Using device %s of platform %s", m_clDevice.getInfo().c_str(), + RCLCPP_INFO("Using device %s of platform %s", m_clDevice.getInfo().c_str(), platform.getInfo().c_str()); - ROS_DEBUG(" "); + RCLCPP_DEBUG(" "); // Read kernel file ifstream in(ros::package::getPath("rviz_map_plugin") + CL_RAY_CAST_KERNEL_FILE); std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - ROS_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_map_plugin").c_str(), CL_RAY_CAST_KERNEL_FILE); + RCLCPP_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_map_plugin").c_str(), CL_RAY_CAST_KERNEL_FILE); m_clProgramSources = cl::Program::Sources(1, { cast_rays_kernel.c_str(), cast_rays_kernel.length() }); @@ -206,11 +206,11 @@ void ClusterLabelTool::onInitialize() try { m_clProgram.build({ m_clDevice }); - ROS_INFO("Successfully built program."); + RCLCPP_INFO("Successfully built program."); } catch (cl::Error& err) { - ROS_ERROR("Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); + RCLCPP_ERROR("Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); ros::shutdown(); exit(1); @@ -221,8 +221,8 @@ void ClusterLabelTool::onInitialize() } catch (cl::Error err) { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); + RCLCPP_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); + RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); ros::requestShutdown(); exit(1); } @@ -318,8 +318,8 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) } catch (cl::Error err) { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); + RCLCPP_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); + RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); ros::shutdown(); exit(1); } @@ -421,8 +421,8 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume } catch (cl::Error err) { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); + RCLCPP_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); + RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); } for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) @@ -482,8 +482,8 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) } catch (cl::Error err) { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); + RCLCPP_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); + RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); } int closestFaceId = -1; @@ -518,7 +518,7 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) m_visual->setFacesInCluster(tmpFaceList); - ROS_DEBUG("selectSingleFaceParallel() found face with id %d", closestFaceId); + RCLCPP_DEBUG("selectSingleFaceParallel() found face with id %d", closestFaceId); } } @@ -553,8 +553,8 @@ void ClusterLabelTool::selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode } catch (cl::Error err) { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); + RCLCPP_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); + RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); } for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) @@ -605,8 +605,8 @@ boost::optional> ClusterLabelTool::getClosestIntersec } catch (cl::Error err) { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); + RCLCPP_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); + RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); } uint32_t closestFaceId; @@ -635,7 +635,7 @@ boost::optional> ClusterLabelTool::getClosestIntersec void ClusterLabelTool::publishLabel(std::string label) { - ROS_DEBUG_STREAM("Label Tool: Publish label '" << label << "'"); + RCLCPP_DEBUG_STREAM("Label Tool: Publish label '" << label << "'"); vector faces; for (uint32_t i = 0; i < m_faceSelectedArray.size(); i++) diff --git a/rviz_map_plugin/src/ClusterLabelVisual.cpp b/rviz_map_plugin/src/ClusterLabelVisual.cpp index e636aaf..327e28b 100644 --- a/rviz_map_plugin/src/ClusterLabelVisual.cpp +++ b/rviz_map_plugin/src/ClusterLabelVisual.cpp @@ -163,7 +163,7 @@ ClusterLabelVisual::~ClusterLabelVisual() if (!m_mesh.isNull()) { - ROS_DEBUG("ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); + RCLCPP_DEBUG("ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); try { @@ -171,13 +171,13 @@ ClusterLabelVisual::~ClusterLabelVisual() } catch (...) { - ROS_ERROR("Exception in Visual destructor"); + RCLCPP_ERROR("Exception in Visual destructor"); } } if (m_sceneNode->numAttachedObjects() == 0) { - ROS_INFO("ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); + RCLCPP_INFO("ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); } } @@ -201,7 +201,7 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) if (!m_geometry) { - ROS_WARN("ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); + RCLCPP_WARN("ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); return; } @@ -212,7 +212,7 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) m_subMesh->indexData->indexCount = 0; m_subMesh->indexData->indexStart = 0; m_material->getTechnique(0)->removeAllPasses(); - ROS_DEBUG("ClusterLabelVisual::setFacesInCluster: faces empty!"); + RCLCPP_DEBUG("ClusterLabelVisual::setFacesInCluster: faces empty!"); return; } diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp index a0298d4..0c1b842 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_map_plugin/src/MapDisplay.cpp @@ -85,7 +85,7 @@ std::shared_ptr MapDisplay::getGeometry() { if (!m_geometry) { - ROS_ERROR("Map Display: Geometry requested, but none available!"); + RCLCPP_ERROR("Map Display: Geometry requested, but none available!"); } return m_geometry; } @@ -244,33 +244,33 @@ bool MapDisplay::loadData() std::string mapFile = m_mapFilePath->getFilename(); if (mapFile.empty()) { - ROS_WARN_STREAM("Map Display: No map file path specified!"); + RCLCPP_WARN_STREAM("Map Display: No map file path specified!"); setStatus(rviz_common::StatusProperty::Warn, "Map", "No map file path specified!"); return false; } if (!boost::filesystem::exists(mapFile)) { - ROS_WARN_STREAM("Map Display: Specified map file does not exist!"); + RCLCPP_WARN_STREAM("Map Display: Specified map file does not exist!"); setStatus(rviz_common::StatusProperty::Warn, "Map", "Specified map file does not exist!"); return false; } - ROS_INFO_STREAM("Map Display: Loading data for map '" << mapFile << "'"); + RCLCPP_INFO_STREAM("Map Display: Loading data for map '" << mapFile << "'"); try { if (boost::filesystem::extension(mapFile).compare(".h5") == 0) { - ROS_INFO("Map Display: Load HDF5 map"); + RCLCPP_INFO("Map Display: Load HDF5 map"); // Open file IO hdf5_map_io::HDF5MapIO map_io(mapFile); - ROS_INFO("Map Display: Load geometry"); + RCLCPP_INFO("Map Display: Load geometry"); // Read geometry m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); - ROS_INFO("Map Display: Load textures"); + RCLCPP_INFO("Map Display: Load textures"); // Read textures vector textures = map_io.getTextures(); @@ -288,7 +288,7 @@ bool MapDisplay::loadData() m_textures[textureIndex].pixelFormat = "rgb8"; } - ROS_INFO("Map Display: Load materials"); + RCLCPP_INFO("Map Display: Load materials"); // Read materials vector materials = map_io.getMaterials(); @@ -322,7 +322,7 @@ bool MapDisplay::loadData() m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); } - ROS_INFO("Map Display: Load vertex colors"); + RCLCPP_INFO("Map Display: Load vertex colors"); // Read vertex colors vector colors = map_io.getVertexColors(); @@ -334,7 +334,7 @@ bool MapDisplay::loadData() m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); } - ROS_INFO("Map Display: Load vertex normals"); + RCLCPP_INFO("Map Display: Load vertex normals"); // Read vertex normals vector normals = map_io.getVertexNormals(); @@ -345,7 +345,7 @@ bool MapDisplay::loadData() m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); } - ROS_INFO("Map Display: Load texture coordinates"); + RCLCPP_INFO("Map Display: Load texture coordinates"); // Read tex cords vector texCoords = map_io.getVertexTextureCoords(); @@ -356,7 +356,7 @@ bool MapDisplay::loadData() m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); } - ROS_INFO("Map Display: Load clusters"); + RCLCPP_INFO("Map Display: Load clusters"); // Read labels m_clusterList.clear(); @@ -384,7 +384,7 @@ bool MapDisplay::loadData() } catch (const hf::DataSpaceException& e) { - ROS_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); + RCLCPP_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); } } } @@ -438,14 +438,14 @@ bool MapDisplay::loadData() } catch (...) { - ROS_ERROR_STREAM("An unexpected error occurred while using Pluto Map IO"); + RCLCPP_ERROR_STREAM("An unexpected error occurred while using Pluto Map IO"); setStatus(rviz_common::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); return false; } setStatus(rviz_common::StatusProperty::Ok, "IO", ""); - ROS_INFO("Map Display: Successfully loaded map."); + RCLCPP_INFO("Map Display: Successfully loaded map."); return true; } @@ -458,7 +458,7 @@ void MapDisplay::saveLabel(Cluster cluster) std::string label = cluster.name; std::vector faces = cluster.faces; - ROS_INFO_STREAM("Map Display: add label '" << label << "'"); + RCLCPP_INFO_STREAM("Map Display: add label '" << label << "'"); try { @@ -467,7 +467,7 @@ void MapDisplay::saveLabel(Cluster cluster) boost::split(results, label, [](char c) { return c == '_'; }); if (results.size() != 2) { - ROS_ERROR_STREAM("Map Display: Illegal label name '" << label << "'"); + RCLCPP_ERROR_STREAM("Map Display: Illegal label name '" << label << "'"); setStatus(rviz_common::StatusProperty::Error, "Label", "Illegal label name!"); return; } @@ -482,7 +482,7 @@ void MapDisplay::saveLabel(Cluster cluster) m_clusterList.push_back(Cluster(label, faces)); setStatus(rviz_common::StatusProperty::Ok, "Label", "Successfully saved label"); - ROS_INFO_STREAM("Map Display: Successfully added label to map."); + RCLCPP_INFO_STREAM("Map Display: Successfully added label to map."); // update the map to show the new label updateMap(); diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index 3dacc2a..1fcc0fd 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -75,19 +75,20 @@ namespace rviz_map_plugin MeshDisplay::MeshDisplay() : rviz_common::Display() , m_ignoreMsgs(false) +, m_node() { // mesh topic - m_meshTopic = new rviz_common::RosTopicProperty( + m_meshTopic = new rviz_common::properties::RosTopicProperty( "Geometry Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized - m_bufferSize = new rviz_common::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); + m_bufferSize = new rviz_common::properties::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); m_bufferSize->setMin(1); // Display Type { - m_displayType = new rviz_common::EnumProperty("Display Type", "Fixed Color", "Select Display Type for Mesh", this, + m_displayType = new rviz_common::properties::EnumProperty("Display Type", "Fixed Color", "Select Display Type for Mesh", this, SLOT(updateMesh()), this); m_displayType->addOption("Fixed Color", 0); m_displayType->addOption("Vertex Color", 1); @@ -98,11 +99,11 @@ MeshDisplay::MeshDisplay() // Fixed Color { // face color properties - m_facesColor = new rviz_common::ColorProperty("Faces Color", QColor(0, 255, 0), "The color of the faces.", m_displayType, + m_facesColor = new rviz_common::properties::ColorProperty("Faces Color", QColor(0, 255, 0), "The color of the faces.", m_displayType, SLOT(updateMesh()), this); // face alpha properties - m_facesAlpha = new rviz_common::FloatProperty("Faces Alpha", 1.0, "The alpha-value of the faces", m_displayType, + m_facesAlpha = new rviz_common::properties::FloatProperty("Faces Alpha", 1.0, "The alpha-value of the faces", m_displayType, SLOT(updateMesh()), this); m_facesAlpha->setMin(0); m_facesAlpha->setMax(1); @@ -110,12 +111,12 @@ MeshDisplay::MeshDisplay() // Vertex Color { - m_vertexColorsTopic = new rviz_common::RosTopicProperty( + m_vertexColorsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Colors Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Vertex color topic to subscribe to.", m_displayType, SLOT(updateVertexColorsTopic()), this); - m_vertexColorServiceName = new rviz_common::StringProperty("Vertex Color Service Name", "get_vertex_colors", + m_vertexColorServiceName = new rviz_common::properties::StringProperty("Vertex Color Service Name", "get_vertex_colors", "Name of the Vertex Color Service to request Vertex Colors " "from.", m_displayType, SLOT(updateVertexColorService()), this); @@ -123,48 +124,48 @@ MeshDisplay::MeshDisplay() // Textures { - m_showTexturedFacesOnly = new rviz_common::BoolProperty("Show textured faces only", false, "Show textured faces only", + m_showTexturedFacesOnly = new rviz_common::properties::BoolProperty("Show textured faces only", false, "Show textured faces only", m_displayType, SLOT(updateMesh()), this); - m_materialServiceName = new rviz_common::StringProperty("Material Service Name", "get_materials", + m_materialServiceName = new rviz_common::properties::StringProperty("Material Service Name", "get_materials", "Name of the Matrial Service to request Materials from.", m_displayType, SLOT(updateMaterialAndTextureServices()), this); - m_textureServiceName = new rviz_common::StringProperty("Texture Service Name", "get_texture", + m_textureServiceName = new rviz_common::properties::StringProperty("Texture Service Name", "get_texture", "Name of the Texture Service to request Textures from.", m_displayType, SLOT(updateMaterialAndTextureServices()), this); } // Vertex Costs { - m_costColorType = new rviz_common::EnumProperty("Color Scale", "Rainbow", + m_costColorType = new rviz_common::properties::EnumProperty("Color Scale", "Rainbow", "Select color scale for vertex costs. Mesh will update when new data " "arrives.", m_displayType, SLOT(updateVertexCosts()), this); m_costColorType->addOption("Rainbow", 0); m_costColorType->addOption("Red Green", 1); - m_vertexCostsTopic = new rviz_common::RosTopicProperty( + m_vertexCostsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Costs Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Vertex cost topic to subscribe to.", m_displayType, SLOT(updateVertexCostsTopic()), this); - m_selectVertexCostMap = new rviz_common::EnumProperty("Vertex Costs Type", "-- None --", + m_selectVertexCostMap = new rviz_common::properties::EnumProperty("Vertex Costs Type", "-- None --", "Select the type of vertex cost map to be displayed. New types " "will appear here when a new message arrives.", m_displayType, SLOT(updateVertexCosts()), this); m_selectVertexCostMap->addOption("-- None --", 0); - m_costUseCustomLimits = new rviz_common::BoolProperty("Use Custom limits", false, "Use custom vertex cost limits", + m_costUseCustomLimits = new rviz_common::properties::BoolProperty("Use Custom limits", false, "Use custom vertex cost limits", m_displayType, SLOT(updateVertexCosts()), this); // custom cost limits { - m_costLowerLimit = new rviz_common::FloatProperty("Vertex Costs Lower Limit", 0.0, "Vertex costs lower limit", + m_costLowerLimit = new rviz_common::properties::FloatProperty("Vertex Costs Lower Limit", 0.0, "Vertex costs lower limit", m_costUseCustomLimits, SLOT(updateVertexCosts()), this); m_costLowerLimit->hide(); - m_costUpperLimit = new rviz_common::FloatProperty("Vertex Costs Upper Limit", 1.0, "Vertex costs upper limit", + m_costUpperLimit = new rviz_common::properties::FloatProperty("Vertex Costs Upper Limit", 1.0, "Vertex costs upper limit", m_costUseCustomLimits, SLOT(updateVertexCosts()), this); m_costUpperLimit->hide(); } @@ -174,13 +175,13 @@ MeshDisplay::MeshDisplay() // Wireframe { m_showWireframe = - new rviz_common::BoolProperty("Show Wireframe", true, "Show Wireframe", this, SLOT(updateWireframe()), this); + new rviz_common::properties::BoolProperty("Show Wireframe", true, "Show Wireframe", this, SLOT(updateWireframe()), this); // wireframe color property - m_wireframeColor = new rviz_common::ColorProperty("Wireframe Color", QColor(0, 0, 0), "The color of the wireframe.", + m_wireframeColor = new rviz_common::properties::ColorProperty("Wireframe Color", QColor(0, 0, 0), "The color of the wireframe.", m_showWireframe, SLOT(updateWireframe()), this); // wireframe alpha property - m_wireframeAlpha = new rviz_common::FloatProperty("Wireframe Alpha", 1.0, "The alpha-value of the wireframe", + m_wireframeAlpha = new rviz_common::properties::FloatProperty("Wireframe Alpha", 1.0, "The alpha-value of the wireframe", m_showWireframe, SLOT(updateWireframe()), this); m_wireframeAlpha->setMin(0); m_wireframeAlpha->setMax(1); @@ -188,15 +189,15 @@ MeshDisplay::MeshDisplay() // Normals { - m_showNormals = new rviz_common::BoolProperty("Show Normals", true, "Show Normals", this, SLOT(updateNormals()), this); + m_showNormals = new rviz_common::properties::BoolProperty("Show Normals", true, "Show Normals", this, SLOT(updateNormals()), this); - m_normalsColor = new rviz_common::ColorProperty("Normals Color", QColor(255, 0, 255), "The color of the normals.", + m_normalsColor = new rviz_common::properties::ColorProperty("Normals Color", QColor(255, 0, 255), "The color of the normals.", m_showNormals, SLOT(updateNormalsColor()), this); - m_normalsAlpha = new rviz_common::FloatProperty("Normals Alpha", 1.0, "The alpha-value of the normals", m_showNormals, + m_normalsAlpha = new rviz_common::properties::FloatProperty("Normals Alpha", 1.0, "The alpha-value of the normals", m_showNormals, SLOT(updateNormalsColor()), this); m_normalsAlpha->setMin(0); m_normalsAlpha->setMax(1); - m_scalingFactor = new rviz_common::FloatProperty("Normals Scaling Factor", 0.1, "Scaling factor of the normals", + m_scalingFactor = new rviz_common::properties::FloatProperty("Normals Scaling Factor", 0.1, "Scaling factor of the normals", m_showNormals, SLOT(updateNormalsSize()), this); } } @@ -230,7 +231,8 @@ void MeshDisplay::onInitialize() m_costsSynchronizer = 0; // Initialize service clients - ros::NodeHandle n; + // ros::NodeHandle n; + // m_node = m_vertexColorClient = n.serviceClient(m_vertexColorServiceName->getStdString()); m_materialsClient = n.serviceClient(m_materialServiceName->getStdString()); @@ -429,7 +431,7 @@ void MeshDisplay::updateBufferSize() void MeshDisplay::updateMesh() { - ROS_INFO("Mesh Display: Update"); + RCLCPP_INFO("Mesh Display: Update"); bool showFaces = false; bool showTextures = false; @@ -514,7 +516,7 @@ void MeshDisplay::updateMesh() std::shared_ptr visual = getLatestVisual(); if (!visual) { - ROS_ERROR("Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); return; } @@ -713,7 +715,7 @@ void MeshDisplay::initialServiceCall() { std::string uuid = uuids[0]; - ROS_INFO_STREAM("Initial data available for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Initial data available for UUID=" << uuid); m_geometryClient = n.serviceClient("get_geometry"); @@ -721,20 +723,20 @@ void MeshDisplay::initialServiceCall() srv_geometry.request.uuid = uuid; if (m_geometryClient.call(srv_geometry)) { - ROS_INFO_STREAM("Found geometry for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Found geometry for UUID=" << uuid); mesh_msgs::MeshGeometryStamped::ConstPtr geometry = boost::make_shared(srv_geometry.response.mesh_geometry_stamped); processMessage(geometry); } else { - ROS_INFO_STREAM("Could not load geometry. Waiting for callback to trigger ... "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); } } } else { - ROS_INFO("No initial data available, waiting for callback to trigger ..."); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "No initial data available, waiting for callback to trigger ..."); } } @@ -751,14 +753,14 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& if (!context_->getFrameManager()->getTransform(meshMsg->header.frame_id, meshMsg->header.stamp, position, orientation)) { - ROS_ERROR("Error transforming from frame '%s' to frame '%s'", meshMsg->header.frame_id.c_str(), + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Error transforming from frame '%s' to frame '%s'", meshMsg->header.frame_id.c_str(), qPrintable(rviz_common::Display::fixed_frame_)); return; } if (!m_lastUuid.empty() && meshMsg->uuid.compare(m_lastUuid) != 0) { - ROS_WARN("Received geometry with new UUID!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received geometry with new UUID!"); m_costCache.clear(); m_selectVertexCostMap->clearOptions(); m_selectVertexCostMap->addOption("-- None --", 0); @@ -811,7 +813,7 @@ void MeshDisplay::incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped: { if (colorsStamped->uuid.compare(m_lastUuid) != 0) { - ROS_ERROR("Received vertex colors, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex colors, but UUIDs dont match!"); return; } @@ -829,7 +831,7 @@ void MeshDisplay::incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::C { if (costsStamped->uuid.compare(m_lastUuid) != 0) { - ROS_ERROR("Received vertex costs, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex costs, but UUIDs dont match!"); return; } @@ -848,7 +850,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) srv.request.uuid = uuid; if (m_vertexColorClient.call(srv)) { - ROS_INFO("Successful vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful vertex colors service call!"); mesh_msgs::MeshVertexColorsStamped::ConstPtr meshVertexColors = boost::make_shared(srv.response.mesh_vertex_colors_stamped); @@ -863,7 +865,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) } else { - ROS_INFO("Failed vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Failed vertex colors service call!"); } } @@ -878,7 +880,7 @@ void MeshDisplay::requestMaterials(std::string uuid) srv.request.uuid = uuid; if (m_materialsClient.call(srv)) { - ROS_INFO("Successful materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful materials service call!"); mesh_msgs::MeshMaterialsStamped::ConstPtr meshMaterialsStamped = boost::make_shared(srv.response.mesh_materials_stamped); @@ -920,7 +922,7 @@ void MeshDisplay::requestMaterials(std::string uuid) texSrv.request.texture_index = material.texture_index; if (m_textureClient.call(texSrv)) { - ROS_INFO("Successful texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful texture service call with index %d!", material.texture_index); mesh_msgs::MeshTexture::ConstPtr textureMsg = boost::make_shared(texSrv.response.texture); @@ -935,31 +937,31 @@ void MeshDisplay::requestMaterials(std::string uuid) } else { - ROS_INFO("Failed texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Failed texture service call with index %d!", material.texture_index); } } } } else { - ROS_INFO("Failed materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Failed materials service call!"); } } void MeshDisplay::cacheVertexCosts(std::string layer, const std::vector& costs) { - ROS_INFO_STREAM("Cache vertex cost map '" << layer << "' for UUID "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Cache vertex cost map '" << layer << "' for UUID "); // insert into cache auto it = m_costCache.find(layer); if (it != m_costCache.end()) { - ROS_INFO_STREAM("The cost layer \"" << layer << "\" has been updated."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "The cost layer \"" << layer << "\" has been updated."); m_costCache.erase(layer); } else { - ROS_INFO_STREAM("The cost layer \"" << layer << "\" has been added."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "The cost layer \"" << layer << "\" has been added."); m_selectVertexCostMap->addOptionStd(layer, m_selectVertexCostMap->numChildren()); } diff --git a/rviz_map_plugin/src/MeshPoseTool.cpp b/rviz_map_plugin/src/MeshPoseTool.cpp index 26f1bbd..7981127 100644 --- a/rviz_map_plugin/src/MeshPoseTool.cpp +++ b/rviz_map_plugin/src/MeshPoseTool.cpp @@ -92,7 +92,7 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) if (event.leftDown()) { - ROS_ASSERT(state_ == Position); + RCLCPP_ASSERT(state_ == Position); Ogre::Vector3 pos, ori; if (selectTriangle(event, pos, ori)) diff --git a/rviz_map_plugin/src/MeshVisual.cpp b/rviz_map_plugin/src/MeshVisual.cpp index 1e2102e..45e0cd3 100644 --- a/rviz_map_plugin/src/MeshVisual.cpp +++ b/rviz_map_plugin/src/MeshVisual.cpp @@ -104,7 +104,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s , m_texture_coords_enabled(false) , m_normalsScalingFactor(1) { - ROS_INFO("Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO("Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); // get or create the scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); @@ -115,12 +115,12 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s std::string sceneId = strstream.str(); if (sceneManager->hasSceneNode(sceneId)) { - // ROS_INFO("Attaching to scene: %s", sceneId); + // RCLCPP_INFO("Attaching to scene: %s", sceneId); m_sceneNode = (Ogre::SceneNode*)(rootNode->getChild(sceneId)); } else { - // ROS_INFO("Creating new scene: %s", sceneId); + // RCLCPP_INFO("Creating new scene: %s", sceneId); m_sceneNode = rootNode->createChildSceneNode(sceneId); } @@ -158,7 +158,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s MeshVisual::~MeshVisual() { - ROS_INFO("Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO("Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); reset(); @@ -189,7 +189,7 @@ MeshVisual::~MeshVisual() void MeshVisual::reset() { - ROS_INFO("Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO("Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); std::stringstream sstm; @@ -554,7 +554,7 @@ void MeshVisual::enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const float range = maxCost - minCost; if (range <= 0) { - ROS_ERROR("Illegal vertex cost limits!"); + RCLCPP_ERROR("Illegal vertex cost limits!"); return; } @@ -651,7 +651,7 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector // if the image was only default constructed, in which case its width will be 0 if (m_images.size() < textureIndex + 1 || m_images[textureIndex].getWidth() == 0) { - ROS_DEBUG("Texture with index %u not loaded yet", textureIndex); + RCLCPP_DEBUG("Texture with index %u not loaded yet", textureIndex); } else { @@ -780,7 +780,7 @@ bool MeshVisual::setGeometry(const Geometry& mesh) // check if there are enough vertices given if (mesh.vertices.size() < 3) { - ROS_WARN("Received not enough vertices, can't create mesh!"); + RCLCPP_WARN("Received not enough vertices, can't create mesh!"); return false; } @@ -804,12 +804,12 @@ bool MeshVisual::setNormals(const vector& normals) // check if there are vertex normals for each vertex if (normals.size() == m_geometry.vertices.size()) { - ROS_INFO("Received %lu vertex normals.", normals.size()); + RCLCPP_INFO("Received %lu vertex normals.", normals.size()); m_vertex_normals_enabled = true; } else if (normals.size() > 0) { - ROS_WARN("Received not as much vertex normals as vertices, ignoring vertex normals!"); + RCLCPP_WARN("Received not as much vertex normals as vertices, ignoring vertex normals!"); return false; } @@ -833,12 +833,12 @@ bool MeshVisual::setVertexColors(const vector& vertexColors) // check if there are vertex colors for each vertex if (vertexColors.size() == m_geometry.vertices.size()) { - ROS_INFO("Received %lu vertex colors.", vertexColors.size()); + RCLCPP_INFO("Received %lu vertex colors.", vertexColors.size()); m_vertex_colors_enabled = true; } else { - ROS_WARN("Received not as much vertex colors as vertices, ignoring the vertex colors!"); + RCLCPP_WARN("Received not as much vertex colors as vertices, ignoring the vertex colors!"); return false; } @@ -857,19 +857,19 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // // check if these MeshVertexCosts belong to the current mesh and were not already loaded // if (m_meshUuid != vertexCostsMsg->uuid) // { - // ROS_WARN("Can't add vertex costs, uuids do not match."); + // RCLCPP_WARN("Can't add vertex costs, uuids do not match."); // return false; // } // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - ROS_DEBUG("Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG("Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - ROS_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -886,19 +886,19 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // // check if these MeshVertexCosts belong to the current mesh and were not already loaded // if (m_meshUuid != vertexCostsMsg->uuid) // { - // ROS_WARN("Can't add vertex costs, uuids do not match."); + // RCLCPP_WARN("Can't add vertex costs, uuids do not match."); // return false; // } // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - ROS_DEBUG("Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG("Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - ROS_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -914,12 +914,12 @@ bool MeshVisual::setMaterials(const vector& materials, const vector= 0) { - ROS_INFO("Received %lu materials.", materials.size()); + RCLCPP_INFO("Received %lu materials.", materials.size()); m_materials_enabled = true; // enable materials } else { - ROS_WARN("Received zero materials, ignoring materials!"); + RCLCPP_WARN("Received zero materials, ignoring materials!"); return false; } @@ -927,13 +927,13 @@ bool MeshVisual::setMaterials(const vector& materials, const vector 0) { - ROS_WARN("Received not as much texture coords as vertices, ignoring texture coords!"); + RCLCPP_WARN("Received not as much texture coords as vertices, ignoring texture coords!"); } enteringTexturedTriangleMesh(m_geometry, materials, texCoords); @@ -962,7 +962,7 @@ bool MeshVisual::addTexture(Texture& texture, uint32_t textureIndex) } else { - ROS_WARN("Can't load image into texture material, material does not exist!"); + RCLCPP_WARN("Can't load image into texture material, material does not exist!"); return false; } } @@ -978,7 +978,7 @@ Ogre::PixelFormat MeshVisual::getOgrePixelFormatFromRosString(std::string encodi return Ogre::PF_BYTE_RGB; } - ROS_WARN("Unknown texture encoding! Using Ogre::PF_UNKNOWN"); + RCLCPP_WARN("Unknown texture encoding! Using Ogre::PF_UNKNOWN"); return Ogre::PF_UNKNOWN; } From 9a79358671a58c7f10b8997a9a4ec4a766cb1fab Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 15:10:20 +0100 Subject: [PATCH 17/40] fixes --- rviz_map_plugin/include/MeshDisplay.hpp | 21 +-- rviz_map_plugin/src/MeshDisplay.cpp | 180 +++++++++++++----------- 2 files changed, 111 insertions(+), 90 deletions(-) diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_map_plugin/include/MeshDisplay.hpp index 4a13d34..37dd8c1 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_map_plugin/include/MeshDisplay.hpp @@ -298,25 +298,25 @@ private Q_SLOTS: * @brief Sets data for trianglemesh_visual and updates the mesh. * @param meshMsg Message containing geometry information */ - void processMessage(const mesh_msgs::msg::MeshGeometryStamped::ConstPtr& meshMsg); + void processMessage(const mesh_msgs::msg::MeshGeometryStamped& meshMsg); /** * @brief Handler for incoming geometry messages. Validate data and update mesh * @param meshMsg The geometry */ - void incomingGeometry(const mesh_msgs::msg::MeshGeometryStamped::ConstPtr& meshMsg); + void incomingGeometry(const mesh_msgs::msg::MeshGeometryStamped& meshMsg); /** * @brief Handler for incoming vertex color messages. Validate data and update mesh * @param colorsStamped The vertex colors */ - void incomingVertexColors(const mesh_msgs::msg::MeshVertexColorsStamped::ConstPtr& colorsStamped); + void incomingVertexColors(const mesh_msgs::msg::MeshVertexColorsStamped& colorsStamped); /** * @brief Handler for incoming vertex cost messages. Validate data and update mesh * @param costsStamped The vertex costs */ - void incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr& costsStamped); + void incomingVertexCosts(const mesh_msgs::msg::MeshVertexCostsStamped& costsStamped); /** * @brief Requests vertex colors from the specified service @@ -351,22 +351,23 @@ private Q_SLOTS: /// if set to true, ignore incoming messages and do not use services to request materials bool m_ignoreMsgs; - rclcpp::Node m_node; + std::shared_ptr m_node; + /// Client to request the vertex colors - ros::ServiceClient m_vertexColorClient; + rclcpp::Client::SharedPtr m_vertexColorClient; /// Client to request the materials - ros::ServiceClient m_materialsClient; + rclcpp::Client::SharedPtr m_materialsClient; /// Client to request the textures - ros::ServiceClient m_textureClient; + rclcpp::Client::SharedPtr m_textureClient; /// Client to request the UUID - ros::ServiceClient m_uuidClient; + rclcpp::Client::SharedPtr m_uuidClient; /// Client to request the geometry - ros::ServiceClient m_geometryClient; + rclcpp::Client::SharedPtr m_geometryClient; /// Subscriber for meshMsg message_filters::Subscriber m_meshSubscriber; diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index 1fcc0fd..483c9c3 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -69,17 +69,18 @@ #include +using std::placeholders::_1; +using std::placeholders::_2; namespace rviz_map_plugin { MeshDisplay::MeshDisplay() : rviz_common::Display() , m_ignoreMsgs(false) -, m_node() { // mesh topic m_meshTopic = new rviz_common::properties::RosTopicProperty( - "Geometry Topic", "", QString::fromStdString(ros::message_traits::datatype()), + "Geometry Topic", "", QString::fromStdString(ros::message_traits::datatype()), "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized @@ -232,12 +233,15 @@ void MeshDisplay::onInitialize() // Initialize service clients // ros::NodeHandle n; - // m_node = - m_vertexColorClient = n.serviceClient(m_vertexColorServiceName->getStdString()); + m_node = std::make_shared("mesh_display"); // TODO: how to name this? What if multiple MeshDisplay instances exists? + m_vertexColorClient = m_node->create_client(m_vertexColorServiceName->getStdString()); + m_materialsClient = m_node->create_client(m_vertexColorServiceName->getStdString()); + m_textureClient = m_node->create_client(m_vertexColorServiceName->getStdString()); + m_uuidClient = m_node->create_client("get_uuid"); + m_geometryClient = m_node->create_client("get_geometry"); + - m_materialsClient = n.serviceClient(m_materialServiceName->getStdString()); - m_textureClient = n.serviceClient(m_textureServiceName->getStdString()); updateMesh(); updateWireframe(); @@ -644,9 +648,11 @@ void MeshDisplay::updateMaterialAndTextureServices() // Update material and texture service clients ros::NodeHandle n; - m_materialsClient = n.serviceClient(m_materialServiceName->getStdString()); - m_textureClient = n.serviceClient(m_textureServiceName->getStdString()); - if (m_materialsClient.exists()) + + m_materialsClient = m_node->create_service(m_materialServiceName->getStdString()); + m_textureClient = m_node->create_service(m_textureServiceName->getStdString()); + + if(m_materialsClient->wait_for_service(std::chrono::seconds(5))) { requestMaterials(m_lastUuid); if (m_textureClient.exists()) @@ -657,9 +663,8 @@ void MeshDisplay::updateMaterialAndTextureServices() { setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); } - } - else - { + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); } } @@ -703,44 +708,42 @@ void MeshDisplay::initialServiceCall() return; } - ros::NodeHandle n; - m_uuidClient = n.serviceClient("get_uuid"); + auto req_uuids = std::make_shared(); + auto fut_uuids = m_uuidClient->async_send_request(req_uuids); - mesh_msgs::GetUUIDs srv_uuids; - if (m_uuidClient.call(srv_uuids)) + if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_uuids) == rclcpp::executor::FutureReturnCode::SUCCESS) { - std::vector uuids = srv_uuids.response.uuids; + auto res_uuids = fut_uuids.get(); + + std::vector uuids = res_uuids->uuids; - if (uuids.size() > 0) + if(uuids.size() > 0) { std::string uuid = uuids[0]; RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Initial data available for UUID=" << uuid); + + // mesh_msgs::srv::GetGeometry srv_geometry; + auto req_geometry = std::make_shared(); + req_geometry->uuid = uuid; - m_geometryClient = n.serviceClient("get_geometry"); - - mesh_msgs::GetGeometry srv_geometry; - srv_geometry.request.uuid = uuid; - if (m_geometryClient.call(srv_geometry)) + auto fut_geometry = m_geometryClient->async_send_request(req_geometry); + if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_geometry) == rclcpp::executor::FutureReturnCode::SUCCESS) { + auto res_geometry = fut_geometry.get(); RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Found geometry for UUID=" << uuid); - mesh_msgs::MeshGeometryStamped::ConstPtr geometry = - boost::make_shared(srv_geometry.response.mesh_geometry_stamped); - processMessage(geometry); - } - else - { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); + processMessage(srv_geometry.response.mesh_geometry_stamped); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); } } - } - else - { + } else { RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "No initial data available, waiting for callback to trigger ..."); } } -void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg) +void MeshDisplay::processMessage(const mesh_msgs::msg::MeshGeometryStamped& meshMsg) { if (m_ignoreMsgs) { @@ -750,15 +753,15 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& Ogre::Quaternion orientation; Ogre::Vector3 position; - if (!context_->getFrameManager()->getTransform(meshMsg->header.frame_id, meshMsg->header.stamp, position, + if (!context_->getFrameManager()->getTransform(meshMsg.header.frame_id, meshMsg.header.stamp, position, orientation)) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Error transforming from frame '%s' to frame '%s'", meshMsg->header.frame_id.c_str(), + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Error transforming from frame '%s' to frame '%s'", meshMsg.header.frame_id.c_str(), qPrintable(rviz_common::Display::fixed_frame_)); return; } - if (!m_lastUuid.empty() && meshMsg->uuid.compare(m_lastUuid) != 0) + if (!m_lastUuid.empty() && meshMsg.uuid.compare(m_lastUuid) != 0) { RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received geometry with new UUID!"); m_costCache.clear(); @@ -766,11 +769,11 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& m_selectVertexCostMap->addOption("-- None --", 0); } - m_lastUuid = meshMsg->uuid; + m_lastUuid = meshMsg.uuid; // set Geometry std::shared_ptr mesh(std::make_shared()); - for (const geometry_msgs::Point& v : meshMsg->mesh_geometry.vertices) + for (const geometry_msgs::msg::Point& v : meshMsg.mesh_geometry.vertices) { Vertex vertex; vertex.x = v.x; @@ -778,7 +781,7 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& vertex.z = v.z; mesh->vertices.push_back(vertex); } - for (const mesh_msgs::MeshTriangleIndices& f : meshMsg->mesh_geometry.faces) + for (const mesh_msgs::msg::MeshTriangleIndices& f : meshMsg.mesh_geometry.faces) { Face face; face.vertexIndices[0] = f.vertex_indices[0]; @@ -791,34 +794,36 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& // set Normals std::vector normals; - for (const geometry_msgs::Point& n : meshMsg->mesh_geometry.vertex_normals) + for (const geometry_msgs::msg::Point& n : meshMsg.mesh_geometry.vertex_normals) { Normal normal(n.x, n.y, n.z); normals.push_back(normal); } setVertexNormals(normals); - requestVertexColors(meshMsg->uuid); - requestMaterials(meshMsg->uuid); + requestVertexColors(meshMsg.uuid); + requestMaterials(meshMsg.uuid); } -void MeshDisplay::incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg) +void MeshDisplay::incomingGeometry( + const mesh_msgs::msg::MeshGeometryStamped& meshMsg) { m_messagesReceived++; setStatus(rviz_common::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); processMessage(meshMsg); } -void MeshDisplay::incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr& colorsStamped) +void MeshDisplay::incomingVertexColors( + const mesh_msgs::msg::MeshVertexColorsStamped& colorsStamped) { - if (colorsStamped->uuid.compare(m_lastUuid) != 0) + if (colorsStamped.uuid.compare(m_lastUuid) != 0) { RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex colors, but UUIDs dont match!"); return; } std::vector vertexColors; - for (const std_msgs::ColorRGBA c : colorsStamped->mesh_vertex_colors.vertex_colors) + for (const std_msgs::msg::ColorRGBA c : colorsStamped.mesh_vertex_colors.vertex_colors) { Color color(c.r, c.g, c.b, c.a); vertexColors.push_back(color); @@ -827,7 +832,8 @@ void MeshDisplay::incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped: setVertexColors(vertexColors); } -void MeshDisplay::incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr& costsStamped) +void MeshDisplay::incomingVertexCosts( + const mesh_msgs::msg::MeshVertexCostsStamped& costsStamped) { if (costsStamped->uuid.compare(m_lastUuid) != 0) { @@ -846,16 +852,19 @@ void MeshDisplay::requestVertexColors(std::string uuid) return; } - mesh_msgs::GetVertexColors srv; - srv.request.uuid = uuid; - if (m_vertexColorClient.call(srv)) + mesh_msgs::srv::GetVertexColors srv; + auto req_vertex_colors = std::make_shared(); + req_vertex_colors->uuid = uuid; + + auto fut_vertex_colors = m_vertexColorClient->async_send_request(req_vertex_colors); + + if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_vertex_colors) == rclcpp::executor::FutureReturnCode::SUCCESS) { + auto res_vertex_colors = fut_vertex_colors.get(); RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful vertex colors service call!"); - mesh_msgs::MeshVertexColorsStamped::ConstPtr meshVertexColors = - boost::make_shared(srv.response.mesh_vertex_colors_stamped); std::vector vertexColors; - for (const std_msgs::ColorRGBA c : meshVertexColors->mesh_vertex_colors.vertex_colors) + for (const std_msgs::msg::ColorRGBA c : res_vertex_colors.mesh_vertex_colors_stamped.mesh_vertex_colors.vertex_colors) { Color color(c.r, c.g, c.b, c.a); vertexColors.push_back(color); @@ -876,28 +885,34 @@ void MeshDisplay::requestMaterials(std::string uuid) return; } - mesh_msgs::GetMaterials srv; - srv.request.uuid = uuid; - if (m_materialsClient.call(srv)) + mesh_msgs::srv::GetMaterials srv; + auto req_materials = std::make_shared(); + req_materials->uuid = uuid; + + auto fut_materials = m_materialsClient->async_send_request(req_materials); + + if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_materials) == rclcpp::executor::FutureReturnCode::SUCCESS) { + auto res_materials = fut_materials.get(); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful materials service call!"); - mesh_msgs::MeshMaterialsStamped::ConstPtr meshMaterialsStamped = - boost::make_shared(srv.response.mesh_materials_stamped); + const mesh_msgs::msg::MeshMaterialsStamped& meshMaterialsStamped = + res_materials->mesh_materials_stamped; - std::vector materials(meshMaterialsStamped->mesh_materials.materials.size()); - for (int i = 0; i < meshMaterialsStamped->mesh_materials.materials.size(); i++) + std::vector materials(meshMaterialsStamped.mesh_materials.materials.size()); + for (int i = 0; i < meshMaterialsStamped.mesh_materials.materials.size(); i++) { - const mesh_msgs::MeshMaterial& mat = meshMaterialsStamped->mesh_materials.materials[i]; + const mesh_msgs::msg::MeshMaterial& mat = meshMaterialsStamped.mesh_materials.materials[i]; materials[i].textureIndex = mat.texture_index; materials[i].color = Color(mat.color.r, mat.color.g, mat.color.b, mat.color.a); } - for (int i = 0; i < meshMaterialsStamped->mesh_materials.clusters.size(); i++) + for (int i = 0; i < meshMaterialsStamped.mesh_materials.clusters.size(); i++) { - const mesh_msgs::MeshFaceCluster& clu = meshMaterialsStamped->mesh_materials.clusters[i]; + const mesh_msgs::msg::MeshFaceCluster& clu = meshMaterialsStamped.mesh_materials.clusters[i]; - uint32_t materialIndex = meshMaterialsStamped->mesh_materials.cluster_materials[i]; - const mesh_msgs::MeshMaterial& mat = meshMaterialsStamped->mesh_materials.materials[materialIndex]; + uint32_t materialIndex = meshMaterialsStamped.mesh_materials.cluster_materials[i]; + const mesh_msgs::msg::MeshMaterial& mat = meshMaterialsStamped.mesh_materials.materials[materialIndex]; for (uint32_t face_index : clu.face_indices) { @@ -906,34 +921,39 @@ void MeshDisplay::requestMaterials(std::string uuid) } std::vector textureCoords; - for (const mesh_msgs::MeshVertexTexCoords& coord : meshMaterialsStamped->mesh_materials.vertex_tex_coords) + for (const mesh_msgs::msg::MeshVertexTexCoords& coord : meshMaterialsStamped.mesh_materials.vertex_tex_coords) { textureCoords.push_back(TexCoords(coord.u, coord.v)); } setMaterials(materials, textureCoords); - for (mesh_msgs::MeshMaterial material : meshMaterialsStamped->mesh_materials.materials) + for (mesh_msgs::msg::MeshMaterial material : meshMaterialsStamped.mesh_materials.materials) { if (material.has_texture) { - mesh_msgs::GetTexture texSrv; - texSrv.request.uuid = uuid; - texSrv.request.texture_index = material.texture_index; - if (m_textureClient.call(texSrv)) + auto req_texture = std::make_shared(); + + req_texture->uuid = uuid; + req_texture->texture_index = material.texture_index; + + auto fut_texture = m_textureClient->async_send_request(req_texture); + + if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_texture) == rclcpp::executor::FutureReturnCode::SUCCESS) { + auto res_texture = fut_texture.get(); RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful texture service call with index %d!", material.texture_index); - mesh_msgs::MeshTexture::ConstPtr textureMsg = - boost::make_shared(texSrv.response.texture); + const mesh_msgs::msg::MeshTexture& textureMsg = + res_texture->texture; Texture texture; - texture.width = textureMsg->image.width; - texture.height = textureMsg->image.height; - texture.channels = textureMsg->image.step; - texture.pixelFormat = textureMsg->image.encoding; - texture.data = textureMsg->image.data; + texture.width = textureMsg.image.width; + texture.height = textureMsg.image.height; + texture.channels = textureMsg.image.step; + texture.pixelFormat = textureMsg.image.encoding; + texture.data = textureMsg.image.data; - addTexture(texture, textureMsg->texture_index); + addTexture(texture, textureMsg.texture_index); } else { From 6e151f742ea535710c06601391bb83b42540cbc0 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 16:23:07 +0100 Subject: [PATCH 18/40] mesh display compiles? --- rviz_map_plugin/src/MeshDisplay.cpp | 61 +++++++++++++++-------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index 483c9c3..6751d64 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -69,6 +69,9 @@ #include +#include "rclcpp/executor.hpp" +#include "rmw/rmw.h" + using std::placeholders::_1; using std::placeholders::_2; @@ -205,6 +208,7 @@ MeshDisplay::MeshDisplay() MeshDisplay::~MeshDisplay() { + unsubscribe(); } void MeshDisplay::onInitialize() @@ -278,11 +282,11 @@ void MeshDisplay::subscribe() m_meshSubscriber.subscribe(update_nh_, m_meshTopic->getTopicStd(), 1); m_vertexColorsSubscriber.subscribe(update_nh_, m_vertexColorsTopic->getTopicStd(), 1); m_vertexCostsSubscriber.subscribe(update_nh_, m_vertexCostsTopic->getTopicStd(), 4); - setStatus(rviz_common::StatusProperty::Ok, "Topic", "OK"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); } catch (ros::Exception& e) { - setStatus(rviz_common::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); } // Nothing @@ -292,14 +296,14 @@ void MeshDisplay::subscribe() } else { - m_meshSynchronizer = new message_filters::Cache(m_meshSubscriber, 10); - m_meshSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingGeometry, this, _1)); + m_meshSynchronizer = new message_filters::Cache(m_meshSubscriber, 10); + m_meshSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingGeometry, this, _1)); - m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); - m_colorsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexColors, this, _1)); + m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); + m_colorsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexColors, this, _1)); - m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); - m_costsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexCosts, this, _1)); + m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); + m_costsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexCosts, this, _1)); } initialServiceCall(); @@ -355,7 +359,7 @@ void MeshDisplay::setGeometry(shared_ptr geometry) updateNormals(); updateWireframe(); } - setStatus(rviz_common::StatusProperty::Ok, "Display", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); } void MeshDisplay::setVertexColors(vector& vertexColors) @@ -435,7 +439,7 @@ void MeshDisplay::updateBufferSize() void MeshDisplay::updateMesh() { - RCLCPP_INFO("Mesh Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Mesh Display: Update"); bool showFaces = false; bool showTextures = false; @@ -638,17 +642,14 @@ void MeshDisplay::updateMaterialAndTextureServices() } // Check if the service names are valid - std::string error; - if (!ros::names::validate(m_materialServiceName->getStdString(), error) || - !ros::names::validate(m_textureServiceName->getStdString(), error)) + if (rmw_validate_full_topic_name(m_materialServiceName->getStdString(), nullptr, nullptr) != RMW_TOPIC_VALID || + rmw_validate_full_topic_name(m_textureServiceName->getStdString(), nullptr, nullptr) != RMW_TOPIC_VALID ) { - setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } // Update material and texture service clients - ros::NodeHandle n; - m_materialsClient = m_node->create_service(m_materialServiceName->getStdString()); m_textureClient = m_node->create_service(m_textureServiceName->getStdString()); @@ -657,15 +658,15 @@ void MeshDisplay::updateMaterialAndTextureServices() requestMaterials(m_lastUuid); if (m_textureClient.exists()) { - setStatus(rviz_common::StatusProperty::Ok, "Services", "Material and Texture Service OK"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Services", "Material and Texture Service OK"); } else { - setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); } } else { RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); - setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); } } @@ -678,23 +679,23 @@ void MeshDisplay::updateVertexColorService() // Check if the service name is valid std::string error; - if (!ros::names::validate(m_vertexColorServiceName->getStdString(), error)) - { - setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); + + if (rmw_validate_full_topic_name(m_vertexColorServiceName->getStdString(), nullptr, nullptr) != RMW_TOPIC_VALID) + { + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } // Update vertex color service client - ros::NodeHandle n; - m_vertexColorClient = n.serviceClient(m_vertexColorServiceName->getStdString()); - if (m_vertexColorClient.exists()) + m_vertexColorClient = m_node->create_client(m_vertexColorServiceName->getStdString()); + if(m_vertexColorClient->wait_for_service(std::chrono::seconds(5))) { - setStatus(rviz_common::StatusProperty::Ok, "Services", "Vertex Color Service OK"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Services", "Vertex Color Service OK"); requestVertexColors(m_lastUuid); } else { - setStatus(rviz_common::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist.")); } } @@ -809,7 +810,7 @@ void MeshDisplay::incomingGeometry( const mesh_msgs::msg::MeshGeometryStamped& meshMsg) { m_messagesReceived++; - setStatus(rviz_common::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); processMessage(meshMsg); } @@ -835,13 +836,13 @@ void MeshDisplay::incomingVertexColors( void MeshDisplay::incomingVertexCosts( const mesh_msgs::msg::MeshVertexCostsStamped& costsStamped) { - if (costsStamped->uuid.compare(m_lastUuid) != 0) + if (costsStamped.uuid.compare(m_lastUuid) != 0) { RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex costs, but UUIDs dont match!"); return; } - cacheVertexCosts(costsStamped->type, costsStamped->mesh_vertex_costs.costs); + cacheVertexCosts(costsStamped.type, costsStamped.mesh_vertex_costs.costs); updateVertexCosts(); } From f2ba4b1ec936ac2796069b6fccefffd451e603a7 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 16:56:31 +0100 Subject: [PATCH 19/40] figured out how to convert multithreaded spinning --- .../include/mesh_msgs_hdf5/mesh_msgs_hdf5.h | 2 +- mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp | 21 ++++++++++++------- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h index 5e6bffc..cc5a1b0 100644 --- a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h +++ b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h @@ -35,7 +35,7 @@ class hdf5_to_msg : public rclcpp::Node { public: - hdf5_to_msg(std::string handle_str = "mesh_msgs_hdf5"); + hdf5_to_msg(); protected: void loadAndPublishGeometry(); diff --git a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp index 497b257..35347e0 100644 --- a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp +++ b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp @@ -2,13 +2,17 @@ #include #include +#include "rclcpp/executors.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" + using std::placeholders::_1; using std::placeholders::_2; -namespace mesh_msgs_hdf5 { +namespace mesh_msgs_hdf5 +{ -hdf5_to_msg::hdf5_to_msg(std::string handle_str) -:rclcpp::Node(handle_str) +hdf5_to_msg::hdf5_to_msg() +:rclcpp::Node("mesh_msgs_hdf5") { // TODO: check if this is correct this->declare_parameter("inputFile", "/tmp/map.h5"); @@ -494,11 +498,12 @@ void hdf5_to_msg::callback_clusterLabel( } // namespace mesh_msgs_hdf5 -int main(int argc, char **args) +int main(int argc, char **argv) { - // ros::init(argc, args, "mesh_msgs_hdf5"); - // mesh_msgs_hdf5::hdf5_to_msg hdf5_to_msg; - // ros::MultiThreadedSpinner spinner(4); - // spinner.spin(); + rclcpp::init(argc, argv); + rclcpp::ExecutorOptions opts; + rclcpp::executors::MultiThreadedExecutor executor(opts, 4); + executor.add_node(std::make_shared()); + executor.spin(); return 0; } From 69759d14b5d8200acb30a60b9a9a021d6fbaaeec Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 19:32:45 +0100 Subject: [PATCH 20/40] more changes --- rviz_map_plugin/CMakeLists.txt | 103 ++++++++++------- rviz_map_plugin/include/MeshDisplay.hpp | 29 +++-- rviz_map_plugin/src/MeshDisplay.cpp | 146 +++++++++++++++--------- 3 files changed, 167 insertions(+), 111 deletions(-) diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index f6b15ac..4671602 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) find_package(std_msgs REQUIRED) @@ -34,6 +35,12 @@ if(assimp_FOUND) add_definitions(-DWITH_ASSIMP) endif(assimp_FOUND) +## This setting causes Qt's "MOC" generation to happen automatically. +# set(CMAKE_AUTOMOC ON) +find_package(Qt5 COMPONENTS Core Widgets) +add_definitions(-DQT_NO_KEYWORDS) + + include_directories( include ${catkin_INCLUDE_DIRS} @@ -42,61 +49,69 @@ include_directories( ${OpenCL_INCLUDE_DIRS} ) -## This setting causes Qt's "MOC" generation to happen automatically. -set(CMAKE_AUTOMOC ON) - -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) -## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies -set(QT_LIBRARIES Qt5::Widgets) - - -add_definitions(-DQT_NO_KEYWORDS) - set(SOURCE_FILES - src/ClusterLabelDisplay.cpp - src/ClusterLabelPanel.cpp - src/ClusterLabelTool.cpp - src/ClusterLabelVisual.cpp - src/MapDisplay.cpp - src/MeshDisplay.cpp + # src/ClusterLabelDisplay.cpp + # src/ClusterLabelPanel.cpp + # src/ClusterLabelTool.cpp + # src/ClusterLabelVisual.cpp + # src/MapDisplay.cpp + # src/MeshDisplay.cpp src/MeshVisual.cpp - src/RvizFileProperty.cpp - src/MeshPoseTool.cpp - src/MeshGoalTool.cpp + # src/RvizFileProperty.cpp + # src/MeshPoseTool.cpp + # src/MeshGoalTool.cpp ) + set(HEADER_FILES - include/ClusterLabelDisplay.hpp - include/ClusterLabelPanel.hpp - include/ClusterLabelVisual.hpp - include/MapDisplay.hpp - include/MeshDisplay.hpp + # include/ClusterLabelDisplay.hpp + # include/ClusterLabelPanel.hpp + # include/ClusterLabelVisual.hpp + # include/MapDisplay.hpp + # include/MeshDisplay.hpp include/MeshVisual.hpp - include/ClusterLabelTool.hpp - include/CLUtil.hpp - include/RvizFileProperty.hpp - include/MeshPoseTool.hpp - include/MeshGoalTool.hpp + # include/ClusterLabelTool.hpp + # include/CLUtil.hpp + # include/RvizFileProperty.hpp + # include/MeshPoseTool.hpp + # include/MeshGoalTool.hpp ) -add_library(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES} ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES}) +qt5_wrap_cpp(RVIZ_MAP_PLUGIN_MOC_FILES "include/MeshVisual.hpp") -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} - ${catkin_LIBRARIES} - ${HDF5_LIBRARIES} - ${HDF5_HL_LIBRARIES} + + +add_library(${PROJECT_NAME} SHARED + ${SOURCE_FILES} + ${RVIZ_MAP_PLUGIN_MOC_FILES} +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + ${geometry_msgs_TARGETS} + rclcpp::rclcpp + rviz_common::rviz_common + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay + rviz_rendering::rviz_rendering + ${sensor_msgs_TARGETS} + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} + tf2_ros::tf2_ros + rmw::rmw + ${message_filters_TARGETS} + ${mesh_msgs_TARGETS} ) -ament_target_dependencies(${PROJECT_NAME} - rclcpp - std_msgs - mesh_msgs - rviz_common - rviz_rendering - tf2_ros - pluginlib - message_filters +target_link_libraries(${PROJECT_NAME} PRIVATE + resource_retriever::resource_retriever# + ${HDF5_LIBRARIES} + ${HDF5_HL_LIBRARIES} ) diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_map_plugin/include/MeshDisplay.hpp index 37dd8c1..2f184d7 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_map_plugin/include/MeshDisplay.hpp @@ -55,7 +55,7 @@ #define MESH_DISPLAY_HPP #include -#include +// #include #include #include @@ -68,18 +68,19 @@ #include #include #include +#include + -// #include -// #include #include #include #include -#include +// #include #include #include #include + #ifndef Q_MOC_RUN #include #include @@ -87,7 +88,7 @@ #include -#include +// #include #include #include @@ -96,7 +97,15 @@ #include #include -#endif +#include +#include +#include +#include +#include + +#endif // Q_MOC_RUN + +#include "rclcpp/rclcpp.hpp" namespace rviz_common { @@ -174,7 +183,7 @@ class MeshDisplay : public rviz_common::Display * @brief Set the geometry * @param geometry The geometry */ - void setGeometry(shared_ptr geometry); + void setGeometry(std::shared_ptr geometry); /** * @brief Set the vertex colors @@ -304,19 +313,19 @@ private Q_SLOTS: * @brief Handler for incoming geometry messages. Validate data and update mesh * @param meshMsg The geometry */ - void incomingGeometry(const mesh_msgs::msg::MeshGeometryStamped& meshMsg); + void incomingGeometry(const mesh_msgs::msg::MeshGeometryStamped::ConstSharedPtr& meshMsg); /** * @brief Handler for incoming vertex color messages. Validate data and update mesh * @param colorsStamped The vertex colors */ - void incomingVertexColors(const mesh_msgs::msg::MeshVertexColorsStamped& colorsStamped); + void incomingVertexColors(const mesh_msgs::msg::MeshVertexColorsStamped::ConstSharedPtr& colorsStamped); /** * @brief Handler for incoming vertex cost messages. Validate data and update mesh * @param costsStamped The vertex costs */ - void incomingVertexCosts(const mesh_msgs::msg::MeshVertexCostsStamped& costsStamped); + void incomingVertexCosts(const mesh_msgs::msg::MeshVertexCostsStamped::ConstSharedPtr& costsStamped); /** * @brief Requests vertex colors from the specified service diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index 6751d64..d45cef1 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -53,11 +53,21 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include + + +#include "mesh_msgs/srv/get_vertex_colors.hpp" +#include "mesh_msgs/srv/get_materials.hpp" +#include "mesh_msgs/srv/get_geometry.hpp" +#include "mesh_msgs/srv/get_texture.hpp" +#include "mesh_msgs/srv/get_uui_ds.hpp" + +#include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp" +#include "mesh_msgs/msg/mesh_geometry_stamped.hpp" +// #include "mesh_msgs/msg/mesh_geometry_stamped__type_traits.hpp" + #include #include @@ -70,8 +80,10 @@ #include #include "rclcpp/executor.hpp" -#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +using namespace std::chrono_literals; using std::placeholders::_1; using std::placeholders::_2; @@ -83,7 +95,7 @@ MeshDisplay::MeshDisplay() { // mesh topic m_meshTopic = new rviz_common::properties::RosTopicProperty( - "Geometry Topic", "", QString::fromStdString(ros::message_traits::datatype()), + "Geometry Topic", "", QString::fromStdString(rosidl_generator_traits::data_type()), "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized @@ -117,7 +129,7 @@ MeshDisplay::MeshDisplay() { m_vertexColorsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Colors Topic", "", - QString::fromStdString(ros::message_traits::datatype()), + QString::fromStdString(rosidl_generator_traits::data_type()), "Vertex color topic to subscribe to.", m_displayType, SLOT(updateVertexColorsTopic()), this); m_vertexColorServiceName = new rviz_common::properties::StringProperty("Vertex Color Service Name", "get_vertex_colors", @@ -149,9 +161,11 @@ MeshDisplay::MeshDisplay() m_costColorType->addOption("Rainbow", 0); m_costColorType->addOption("Red Green", 1); + + m_vertexCostsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Costs Topic", "", - QString::fromStdString(ros::message_traits::datatype()), + QString::fromStdString(rosidl_generator_traits::data_type()), "Vertex cost topic to subscribe to.", m_displayType, SLOT(updateVertexCostsTopic()), this); m_selectVertexCostMap = new rviz_common::properties::EnumProperty("Vertex Costs Type", "-- None --", @@ -213,23 +227,33 @@ MeshDisplay::~MeshDisplay() void MeshDisplay::onInitialize() { - m_tfMeshFilter = new tf2_ros::MessageFilter( - *rviz_common::Display::context_->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 2, - rviz_common::Display::update_nh_); - m_tfMeshFilter->connectInput(m_meshSubscriber); - context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfMeshFilter, this); - m_tfVertexColorsFilter = new tf2_ros::MessageFilter( - *rviz_common::Display::context_->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, - rviz_common::Display::update_nh_); - m_tfVertexColorsFilter->connectInput(m_vertexColorsSubscriber); - context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexColorsFilter, this); + // TODO: test this: + // auto tf_wrapper = std::dynamic_pointer_cast( + // context_->getFrameManager()->getConnector().lock()); + // ... + // tf_wrapper->getBuffer(); + + + // context_->getFrameManager()->getTFClientPtr(); - m_tfVertexCostsFilter = new tf2_ros::MessageFilter( - *rviz_common::Display::context_->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, - rviz_common::Display::update_nh_); - m_tfVertexCostsFilter->connectInput(m_vertexCostsSubscriber); - context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexCostsFilter, this); + // m_tfMeshFilter = new tf2_ros::MessageFilter( + // *context_->getFrameManager()->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 2, + // m_node); + // m_tfMeshFilter->connectInput(m_meshSubscriber); + // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfMeshFilter, this); + + // m_tfVertexColorsFilter = new tf2_ros::MessageFilter( + // *context_->getFrameManager()->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, + // m_node); + // m_tfVertexColorsFilter->connectInput(m_vertexColorsSubscriber); + // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexColorsFilter, this); + + // m_tfVertexCostsFilter = new tf2_ros::MessageFilter( + // *context_->getFrameManager()->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, + // m_node); + // m_tfVertexCostsFilter->connectInput(m_vertexCostsSubscriber); + // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexCostsFilter, this); m_meshSynchronizer = 0; m_colorsSynchronizer = 0; @@ -237,7 +261,7 @@ void MeshDisplay::onInitialize() // Initialize service clients // ros::NodeHandle n; - m_node = std::make_shared("mesh_display"); // TODO: how to name this? What if multiple MeshDisplay instances exists? + m_node = rclcpp::Node::make_shared("mesh_display"); // TODO: how to name this? What if multiple MeshDisplay instances exists? m_vertexColorClient = m_node->create_client(m_vertexColorServiceName->getStdString()); m_materialsClient = m_node->create_client(m_vertexColorServiceName->getStdString()); m_textureClient = m_node->create_client(m_vertexColorServiceName->getStdString()); @@ -279,12 +303,16 @@ void MeshDisplay::subscribe() try { - m_meshSubscriber.subscribe(update_nh_, m_meshTopic->getTopicStd(), 1); - m_vertexColorsSubscriber.subscribe(update_nh_, m_vertexColorsTopic->getTopicStd(), 1); - m_vertexCostsSubscriber.subscribe(update_nh_, m_vertexCostsTopic->getTopicStd(), 4); + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 1; + m_meshSubscriber.subscribe(m_node, m_meshTopic->getTopicStd(), qos); + qos.depth = 1; + m_vertexColorsSubscriber.subscribe(m_node, m_vertexColorsTopic->getTopicStd(), qos); + qos.depth = 4; + m_vertexCostsSubscriber.subscribe(m_node, m_vertexCostsTopic->getTopicStd(), qos); setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); } - catch (ros::Exception& e) + catch (std::runtime_error& e) { setStatus(rviz_common::properties::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); } @@ -612,9 +640,11 @@ void MeshDisplay::updateVertexColorsTopic() m_vertexColorsSubscriber.unsubscribe(); delete m_colorsSynchronizer; - m_vertexColorsSubscriber.subscribe(update_nh_, m_vertexColorsTopic->getTopicStd(), 1); - m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); - m_colorsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexColors, this, _1)); + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 1; + m_vertexColorsSubscriber.subscribe(m_node, m_vertexColorsTopic->getTopicStd(), qos); + m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); + m_colorsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexColors, this, _1)); } void MeshDisplay::updateVertexCostsTopic() @@ -622,9 +652,11 @@ void MeshDisplay::updateVertexCostsTopic() m_vertexCostsSubscriber.unsubscribe(); delete m_costsSynchronizer; - m_vertexCostsSubscriber.subscribe(update_nh_, m_vertexCostsTopic->getTopicStd(), 4); - m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); - m_costsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexCosts, this, _1)); + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 4; + m_vertexCostsSubscriber.subscribe(m_node, m_vertexCostsTopic->getTopicStd(), qos); + m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); + m_costsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexCosts, this, _1)); } void MeshDisplay::updateTopic() @@ -642,21 +674,22 @@ void MeshDisplay::updateMaterialAndTextureServices() } // Check if the service names are valid - if (rmw_validate_full_topic_name(m_materialServiceName->getStdString(), nullptr, nullptr) != RMW_TOPIC_VALID || - rmw_validate_full_topic_name(m_textureServiceName->getStdString(), nullptr, nullptr) != RMW_TOPIC_VALID ) + if (rmw_validate_full_topic_name(m_materialServiceName->getStdString().c_str(), nullptr, nullptr) != RMW_TOPIC_VALID || + rmw_validate_full_topic_name(m_textureServiceName->getStdString().c_str(), nullptr, nullptr) != RMW_TOPIC_VALID ) { setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } // Update material and texture service clients - m_materialsClient = m_node->create_service(m_materialServiceName->getStdString()); - m_textureClient = m_node->create_service(m_textureServiceName->getStdString()); + m_materialsClient = m_node->create_client(m_materialServiceName->getStdString()); + m_textureClient = m_node->create_client(m_textureServiceName->getStdString()); if(m_materialsClient->wait_for_service(std::chrono::seconds(5))) { requestMaterials(m_lastUuid); - if (m_textureClient.exists()) + + if (m_textureClient->wait_for_service(1s)) { setStatus(rviz_common::properties::StatusProperty::Ok, "Services", "Material and Texture Service OK"); } @@ -680,7 +713,7 @@ void MeshDisplay::updateVertexColorService() // Check if the service name is valid std::string error; - if (rmw_validate_full_topic_name(m_vertexColorServiceName->getStdString(), nullptr, nullptr) != RMW_TOPIC_VALID) + if (rmw_validate_full_topic_name(m_vertexColorServiceName->getStdString().c_str(), nullptr, nullptr) != RMW_TOPIC_VALID) { setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; @@ -712,7 +745,7 @@ void MeshDisplay::initialServiceCall() auto req_uuids = std::make_shared(); auto fut_uuids = m_uuidClient->async_send_request(req_uuids); - if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_uuids) == rclcpp::executor::FutureReturnCode::SUCCESS) + if(rclcpp::spin_until_future_complete(m_node, fut_uuids) == rclcpp::FutureReturnCode::SUCCESS) { auto res_uuids = fut_uuids.get(); @@ -729,11 +762,11 @@ void MeshDisplay::initialServiceCall() req_geometry->uuid = uuid; auto fut_geometry = m_geometryClient->async_send_request(req_geometry); - if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_geometry) == rclcpp::executor::FutureReturnCode::SUCCESS) + if(rclcpp::spin_until_future_complete(m_node, fut_geometry) == rclcpp::FutureReturnCode::SUCCESS) { auto res_geometry = fut_geometry.get(); RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Found geometry for UUID=" << uuid); - processMessage(srv_geometry.response.mesh_geometry_stamped); + processMessage(res_geometry->mesh_geometry_stamped); } else { RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); @@ -807,24 +840,24 @@ void MeshDisplay::processMessage(const mesh_msgs::msg::MeshGeometryStamped& mesh } void MeshDisplay::incomingGeometry( - const mesh_msgs::msg::MeshGeometryStamped& meshMsg) + const mesh_msgs::msg::MeshGeometryStamped::ConstSharedPtr& meshMsg) { m_messagesReceived++; setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); - processMessage(meshMsg); + processMessage(*meshMsg); } void MeshDisplay::incomingVertexColors( - const mesh_msgs::msg::MeshVertexColorsStamped& colorsStamped) + const mesh_msgs::msg::MeshVertexColorsStamped::ConstSharedPtr& colorsStamped) { - if (colorsStamped.uuid.compare(m_lastUuid) != 0) + if (colorsStamped->uuid.compare(m_lastUuid) != 0) { RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex colors, but UUIDs dont match!"); return; } std::vector vertexColors; - for (const std_msgs::msg::ColorRGBA c : colorsStamped.mesh_vertex_colors.vertex_colors) + for (const std_msgs::msg::ColorRGBA c : colorsStamped->mesh_vertex_colors.vertex_colors) { Color color(c.r, c.g, c.b, c.a); vertexColors.push_back(color); @@ -834,15 +867,15 @@ void MeshDisplay::incomingVertexColors( } void MeshDisplay::incomingVertexCosts( - const mesh_msgs::msg::MeshVertexCostsStamped& costsStamped) + const mesh_msgs::msg::MeshVertexCostsStamped::ConstSharedPtr& costsStamped) { - if (costsStamped.uuid.compare(m_lastUuid) != 0) + if (costsStamped->uuid.compare(m_lastUuid) != 0) { RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex costs, but UUIDs dont match!"); return; } - cacheVertexCosts(costsStamped.type, costsStamped.mesh_vertex_costs.costs); + cacheVertexCosts(costsStamped->type, costsStamped->mesh_vertex_costs.costs); updateVertexCosts(); } @@ -853,19 +886,18 @@ void MeshDisplay::requestVertexColors(std::string uuid) return; } - mesh_msgs::srv::GetVertexColors srv; auto req_vertex_colors = std::make_shared(); req_vertex_colors->uuid = uuid; auto fut_vertex_colors = m_vertexColorClient->async_send_request(req_vertex_colors); - if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_vertex_colors) == rclcpp::executor::FutureReturnCode::SUCCESS) + if(rclcpp::spin_until_future_complete(m_node, fut_vertex_colors) == rclcpp::FutureReturnCode::SUCCESS) { auto res_vertex_colors = fut_vertex_colors.get(); RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful vertex colors service call!"); std::vector vertexColors; - for (const std_msgs::msg::ColorRGBA c : res_vertex_colors.mesh_vertex_colors_stamped.mesh_vertex_colors.vertex_colors) + for (const std_msgs::msg::ColorRGBA c : res_vertex_colors->mesh_vertex_colors_stamped.mesh_vertex_colors.vertex_colors) { Color color(c.r, c.g, c.b, c.a); vertexColors.push_back(color); @@ -892,7 +924,7 @@ void MeshDisplay::requestMaterials(std::string uuid) auto fut_materials = m_materialsClient->async_send_request(req_materials); - if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_materials) == rclcpp::executor::FutureReturnCode::SUCCESS) + if(rclcpp::spin_until_future_complete(m_node, fut_materials) == rclcpp::FutureReturnCode::SUCCESS) { auto res_materials = fut_materials.get(); @@ -940,7 +972,7 @@ void MeshDisplay::requestMaterials(std::string uuid) auto fut_texture = m_textureClient->async_send_request(req_texture); - if(rclcpp::spin_until_future_complete(m_node->get_node_base_interface(), fut_texture) == rclcpp::executor::FutureReturnCode::SUCCESS) + if(rclcpp::spin_until_future_complete(m_node, fut_texture) == rclcpp::FutureReturnCode::SUCCESS) { auto res_texture = fut_texture.get(); RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful texture service call with index %d!", material.texture_index); From bfd0dac3b6858c9638fb830d8b1243d15addad53 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 19:49:20 +0100 Subject: [PATCH 21/40] mesh visual is compiling --- rviz_map_plugin/CMakeLists.txt | 1 + rviz_map_plugin/include/MeshVisual.hpp | 14 +++---- rviz_map_plugin/src/MeshVisual.cpp | 53 ++++++++++++++------------ 3 files changed, 36 insertions(+), 32 deletions(-) diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index 4671602..244d902 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp REQUIRED) find_package(rmw REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) find_package(std_msgs REQUIRED) find_package(hdf5_map_io REQUIRED) find_package(mesh_msgs REQUIRED) diff --git a/rviz_map_plugin/include/MeshVisual.hpp b/rviz_map_plugin/include/MeshVisual.hpp index 1627f6b..6d291dd 100644 --- a/rviz_map_plugin/include/MeshVisual.hpp +++ b/rviz_map_plugin/include/MeshVisual.hpp @@ -69,12 +69,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -82,7 +82,7 @@ namespace Ogre { // Forward declaration -class Vector3; +// class Vector3; class Quaternion; class SceneNode; class Entity; diff --git a/rviz_map_plugin/src/MeshVisual.cpp b/rviz_map_plugin/src/MeshVisual.cpp index 45e0cd3..29057ee 100644 --- a/rviz_map_plugin/src/MeshVisual.cpp +++ b/rviz_map_plugin/src/MeshVisual.cpp @@ -52,15 +52,18 @@ #include "MeshVisual.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include +#include "rclcpp/rclcpp.hpp" + namespace rviz_map_plugin { Ogre::ColourValue getRainbowColor1(float value) @@ -104,7 +107,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s , m_texture_coords_enabled(false) , m_normalsScalingFactor(1) { - RCLCPP_INFO("Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); // get or create the scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); @@ -158,7 +161,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s MeshVisual::~MeshVisual() { - RCLCPP_INFO("Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); reset(); @@ -189,7 +192,7 @@ MeshVisual::~MeshVisual() void MeshVisual::reset() { - RCLCPP_INFO("Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); std::stringstream sstm; @@ -554,7 +557,7 @@ void MeshVisual::enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const float range = maxCost - minCost; if (range <= 0) { - RCLCPP_ERROR("Illegal vertex cost limits!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Illegal vertex cost limits!"); return; } @@ -651,7 +654,7 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector // if the image was only default constructed, in which case its width will be 0 if (m_images.size() < textureIndex + 1 || m_images[textureIndex].getWidth() == 0) { - RCLCPP_DEBUG("Texture with index %u not loaded yet", textureIndex); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_map_plugin"), "Texture with index %u not loaded yet", textureIndex); } else { @@ -780,7 +783,7 @@ bool MeshVisual::setGeometry(const Geometry& mesh) // check if there are enough vertices given if (mesh.vertices.size() < 3) { - RCLCPP_WARN("Received not enough vertices, can't create mesh!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not enough vertices, can't create mesh!"); return false; } @@ -804,12 +807,12 @@ bool MeshVisual::setNormals(const vector& normals) // check if there are vertex normals for each vertex if (normals.size() == m_geometry.vertices.size()) { - RCLCPP_INFO("Received %lu vertex normals.", normals.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex normals.", normals.size()); m_vertex_normals_enabled = true; } else if (normals.size() > 0) { - RCLCPP_WARN("Received not as much vertex normals as vertices, ignoring vertex normals!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex normals as vertices, ignoring vertex normals!"); return false; } @@ -833,12 +836,12 @@ bool MeshVisual::setVertexColors(const vector& vertexColors) // check if there are vertex colors for each vertex if (vertexColors.size() == m_geometry.vertices.size()) { - RCLCPP_INFO("Received %lu vertex colors.", vertexColors.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex colors.", vertexColors.size()); m_vertex_colors_enabled = true; } else { - RCLCPP_WARN("Received not as much vertex colors as vertices, ignoring the vertex colors!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex colors as vertices, ignoring the vertex colors!"); return false; } @@ -864,12 +867,12 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - RCLCPP_DEBUG("Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - RCLCPP_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -893,12 +896,12 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - RCLCPP_DEBUG("Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - RCLCPP_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -914,12 +917,12 @@ bool MeshVisual::setMaterials(const vector& materials, const vector= 0) { - RCLCPP_INFO("Received %lu materials.", materials.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Received %lu materials.", materials.size()); m_materials_enabled = true; // enable materials } else { - RCLCPP_WARN("Received zero materials, ignoring materials!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received zero materials, ignoring materials!"); return false; } @@ -927,13 +930,13 @@ bool MeshVisual::setMaterials(const vector& materials, const vector 0) { - RCLCPP_WARN("Received not as much texture coords as vertices, ignoring texture coords!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much texture coords as vertices, ignoring texture coords!"); } enteringTexturedTriangleMesh(m_geometry, materials, texCoords); @@ -962,7 +965,7 @@ bool MeshVisual::addTexture(Texture& texture, uint32_t textureIndex) } else { - RCLCPP_WARN("Can't load image into texture material, material does not exist!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Can't load image into texture material, material does not exist!"); return false; } } @@ -978,7 +981,7 @@ Ogre::PixelFormat MeshVisual::getOgrePixelFormatFromRosString(std::string encodi return Ogre::PF_BYTE_RGB; } - RCLCPP_WARN("Unknown texture encoding! Using Ogre::PF_UNKNOWN"); + RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Unknown texture encoding! Using Ogre::PF_UNKNOWN"); return Ogre::PF_UNKNOWN; } From ada97651a858c6731ccb517d61c7887b31af3e16 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 20:00:05 +0100 Subject: [PATCH 22/40] mesh display and mesh visual are compiling --- rviz_map_plugin/CMakeLists.txt | 14 +++++++------- rviz_map_plugin/include/MeshDisplay.hpp | 18 +++++++++--------- rviz_map_plugin/include/MeshVisual.hpp | 1 - 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index 244d902..cb55c43 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -56,7 +56,7 @@ set(SOURCE_FILES # src/ClusterLabelTool.cpp # src/ClusterLabelVisual.cpp # src/MapDisplay.cpp - # src/MeshDisplay.cpp + src/MeshDisplay.cpp src/MeshVisual.cpp # src/RvizFileProperty.cpp # src/MeshPoseTool.cpp @@ -64,12 +64,12 @@ set(SOURCE_FILES ) -set(HEADER_FILES +set(MOC_HEADER_FILES # include/ClusterLabelDisplay.hpp # include/ClusterLabelPanel.hpp # include/ClusterLabelVisual.hpp # include/MapDisplay.hpp - # include/MeshDisplay.hpp + include/MeshDisplay.hpp include/MeshVisual.hpp # include/ClusterLabelTool.hpp # include/CLUtil.hpp @@ -78,9 +78,9 @@ set(HEADER_FILES # include/MeshGoalTool.hpp ) -qt5_wrap_cpp(RVIZ_MAP_PLUGIN_MOC_FILES "include/MeshVisual.hpp") - - +foreach(header "${RVIZ_MAP_PLUGIN_MOC_FILES}") + qt5_wrap_cpp(RVIZ_MAP_PLUGIN_MOC_FILES "${header}") +endforeach() add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} @@ -110,7 +110,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PRIVATE - resource_retriever::resource_retriever# + resource_retriever::resource_retriever ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} ) diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_map_plugin/include/MeshDisplay.hpp index 2f184d7..8aac761 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_map_plugin/include/MeshDisplay.hpp @@ -55,7 +55,7 @@ #define MESH_DISPLAY_HPP #include -// #include +#include #include #include @@ -73,7 +73,7 @@ #include #include -#include +// #include // #include #include @@ -88,14 +88,14 @@ #include -// #include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include diff --git a/rviz_map_plugin/include/MeshVisual.hpp b/rviz_map_plugin/include/MeshVisual.hpp index 6d291dd..f4f7f62 100644 --- a/rviz_map_plugin/include/MeshVisual.hpp +++ b/rviz_map_plugin/include/MeshVisual.hpp @@ -82,7 +82,6 @@ namespace Ogre { // Forward declaration -// class Vector3; class Quaternion; class SceneNode; class Entity; From b786ebd65aaace5149d87234d8349367e8ec0a05 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 9 Nov 2023 21:05:44 +0100 Subject: [PATCH 23/40] fixed message filters for new rviz transfomer types. replaced hacky ros node with rviz node interface --- rviz_map_plugin/CMakeLists.txt | 2 + rviz_map_plugin/include/MeshDisplay.hpp | 22 +++-- rviz_map_plugin/include/RVizMessageFilter.hpp | 18 ++++ rviz_map_plugin/include/Types.hpp | 2 + rviz_map_plugin/src/MeshDisplay.cpp | 97 ++++++++++--------- 5 files changed, 90 insertions(+), 51 deletions(-) create mode 100644 rviz_map_plugin/include/RVizMessageFilter.hpp diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt index cb55c43..a9499e6 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_map_plugin/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(rmw REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) find_package(rviz_ogre_vendor REQUIRED) +find_package(rviz_default_plugins REQUIRED) find_package(std_msgs REQUIRED) find_package(hdf5_map_io REQUIRED) find_package(mesh_msgs REQUIRED) @@ -100,6 +101,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor::OgreMain rviz_ogre_vendor::OgreOverlay rviz_rendering::rviz_rendering + rviz_default_plugins::rviz_default_plugins ${sensor_msgs_TARGETS} tf2::tf2 ${tf2_geometry_msgs_TARGETS} diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_map_plugin/include/MeshDisplay.hpp index 8aac761..e270033 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_map_plugin/include/MeshDisplay.hpp @@ -79,6 +79,7 @@ #include #include #include +#include #ifndef Q_MOC_RUN @@ -86,7 +87,7 @@ #include #include -#include +#include #include @@ -141,7 +142,7 @@ class MeshDisplay : public rviz_common::Display Q_OBJECT public: - /** + /**#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" * @brief Constructor */ MeshDisplay(); @@ -360,7 +361,12 @@ private Q_SLOTS: /// if set to true, ignore incoming messages and do not use services to request materials bool m_ignoreMsgs; - std::shared_ptr m_node; + + // TODO: use this instead: + // ros_integration::RosNodeAbstractionIface::WeakPtr m_node; + // from + // #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" + // std::shared_ptr m_node; /// Client to request the vertex colors @@ -388,13 +394,17 @@ private Q_SLOTS: message_filters::Subscriber m_vertexCostsSubscriber; /// Messagefilter for meshMsg - tf2_ros::MessageFilter* m_tfMeshFilter; + + + + + tf2_ros::RVizMessageFilterPtr m_tfMeshFilter; /// Messagefilter for vertex colors - tf2_ros::MessageFilter* m_tfVertexColorsFilter; + tf2_ros::RVizMessageFilterPtr m_tfVertexColorsFilter; /// Messagefilter for vertex costs - tf2_ros::MessageFilter* m_tfVertexCostsFilter; + tf2_ros::RVizMessageFilterPtr m_tfVertexCostsFilter; /// Synchronizer for meshMsgs message_filters::Cache* m_meshSynchronizer; diff --git a/rviz_map_plugin/include/RVizMessageFilter.hpp b/rviz_map_plugin/include/RVizMessageFilter.hpp new file mode 100644 index 0000000..b03656a --- /dev/null +++ b/rviz_map_plugin/include/RVizMessageFilter.hpp @@ -0,0 +1,18 @@ +#ifndef RVIZ_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP +#define RVIZ_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP + +#include +#include + +namespace tf2_ros +{ + +template +using RVizMessageFilter = tf2_ros::MessageFilter; + +template +using RVizMessageFilterPtr = std::shared_ptr >; + +} // namespace tf2_ros + +#endif // RVIZ_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP \ No newline at end of file diff --git a/rviz_map_plugin/include/Types.hpp b/rviz_map_plugin/include/Types.hpp index fde90a9..82cf603 100644 --- a/rviz_map_plugin/include/Types.hpp +++ b/rviz_map_plugin/include/Types.hpp @@ -60,6 +60,8 @@ using std::array; using std::string; using std::vector; + + /// Struct for normals struct Normal { diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_map_plugin/src/MeshDisplay.cpp index d45cef1..4269056 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_map_plugin/src/MeshDisplay.cpp @@ -66,7 +66,6 @@ #include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp" #include "mesh_msgs/msg/mesh_geometry_stamped.hpp" -// #include "mesh_msgs/msg/mesh_geometry_stamped__type_traits.hpp" #include @@ -95,7 +94,7 @@ MeshDisplay::MeshDisplay() { // mesh topic m_meshTopic = new rviz_common::properties::RosTopicProperty( - "Geometry Topic", "", QString::fromStdString(rosidl_generator_traits::data_type()), + "Geometry Topic", "", QString::fromStdString(rosidl_generator_traits::name()), "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized @@ -129,7 +128,7 @@ MeshDisplay::MeshDisplay() { m_vertexColorsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Colors Topic", "", - QString::fromStdString(rosidl_generator_traits::data_type()), + QString::fromStdString(rosidl_generator_traits::name()), "Vertex color topic to subscribe to.", m_displayType, SLOT(updateVertexColorsTopic()), this); m_vertexColorServiceName = new rviz_common::properties::StringProperty("Vertex Color Service Name", "get_vertex_colors", @@ -165,7 +164,7 @@ MeshDisplay::MeshDisplay() m_vertexCostsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Costs Topic", "", - QString::fromStdString(rosidl_generator_traits::data_type()), + QString::fromStdString(rosidl_generator_traits::name()), "Vertex cost topic to subscribe to.", m_displayType, SLOT(updateVertexCostsTopic()), this); m_selectVertexCostMap = new rviz_common::properties::EnumProperty("Vertex Costs Type", "-- None --", @@ -227,32 +226,24 @@ MeshDisplay::~MeshDisplay() void MeshDisplay::onInitialize() { + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - // TODO: test this: - // auto tf_wrapper = std::dynamic_pointer_cast( - // context_->getFrameManager()->getConnector().lock()); - // ... - // tf_wrapper->getBuffer(); - - - // context_->getFrameManager()->getTFClientPtr(); - - // m_tfMeshFilter = new tf2_ros::MessageFilter( - // *context_->getFrameManager()->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 2, - // m_node); - // m_tfMeshFilter->connectInput(m_meshSubscriber); + m_tfMeshFilter = std::make_shared >( + *context_->getFrameManager()->getTransformer(), rviz_common::Display::fixed_frame_.toStdString(), 2, + node); + m_tfMeshFilter->connectInput(m_meshSubscriber); // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfMeshFilter, this); - // m_tfVertexColorsFilter = new tf2_ros::MessageFilter( - // *context_->getFrameManager()->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, - // m_node); - // m_tfVertexColorsFilter->connectInput(m_vertexColorsSubscriber); + m_tfVertexColorsFilter = std::make_shared >( + *context_->getFrameManager()->getTransformer(), rviz_common::Display::fixed_frame_.toStdString(), 10, + node); + m_tfVertexColorsFilter->connectInput(m_vertexColorsSubscriber); // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexColorsFilter, this); - // m_tfVertexCostsFilter = new tf2_ros::MessageFilter( - // *context_->getFrameManager()->getTF2BufferPtr(), rviz_common::Display::fixed_frame_.toStdString(), 10, - // m_node); - // m_tfVertexCostsFilter->connectInput(m_vertexCostsSubscriber); + m_tfVertexCostsFilter = std::make_shared >( + *context_->getFrameManager()->getTransformer(), rviz_common::Display::fixed_frame_.toStdString(), 10, + node); + m_tfVertexCostsFilter->connectInput(m_vertexCostsSubscriber); // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexCostsFilter, this); m_meshSynchronizer = 0; @@ -261,12 +252,16 @@ void MeshDisplay::onInitialize() // Initialize service clients // ros::NodeHandle n; - m_node = rclcpp::Node::make_shared("mesh_display"); // TODO: how to name this? What if multiple MeshDisplay instances exists? - m_vertexColorClient = m_node->create_client(m_vertexColorServiceName->getStdString()); - m_materialsClient = m_node->create_client(m_vertexColorServiceName->getStdString()); - m_textureClient = m_node->create_client(m_vertexColorServiceName->getStdString()); - m_uuidClient = m_node->create_client("get_uuid"); - m_geometryClient = m_node->create_client("get_geometry"); + // m_node = rclcpp::Node::make_shared("mesh_display"); // TODO: how to name this? What if multiple MeshDisplay instances exists? + + // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + + m_vertexColorClient = node->create_client(m_vertexColorServiceName->getStdString()); + m_materialsClient = node->create_client(m_vertexColorServiceName->getStdString()); + m_textureClient = node->create_client(m_vertexColorServiceName->getStdString()); + m_uuidClient = node->create_client("get_uuid"); + m_geometryClient = node->create_client("get_geometry"); @@ -303,13 +298,15 @@ void MeshDisplay::subscribe() try { + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + rmw_qos_profile_t qos = rmw_qos_profile_default; qos.depth = 1; - m_meshSubscriber.subscribe(m_node, m_meshTopic->getTopicStd(), qos); + m_meshSubscriber.subscribe(node, m_meshTopic->getTopicStd(), qos); qos.depth = 1; - m_vertexColorsSubscriber.subscribe(m_node, m_vertexColorsTopic->getTopicStd(), qos); + m_vertexColorsSubscriber.subscribe(node, m_vertexColorsTopic->getTopicStd(), qos); qos.depth = 4; - m_vertexCostsSubscriber.subscribe(m_node, m_vertexCostsTopic->getTopicStd(), qos); + m_vertexCostsSubscriber.subscribe(node, m_vertexCostsTopic->getTopicStd(), qos); setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); } catch (std::runtime_error& e) @@ -640,9 +637,11 @@ void MeshDisplay::updateVertexColorsTopic() m_vertexColorsSubscriber.unsubscribe(); delete m_colorsSynchronizer; + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + rmw_qos_profile_t qos = rmw_qos_profile_default; qos.depth = 1; - m_vertexColorsSubscriber.subscribe(m_node, m_vertexColorsTopic->getTopicStd(), qos); + m_vertexColorsSubscriber.subscribe(node, m_vertexColorsTopic->getTopicStd(), qos); m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); m_colorsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexColors, this, _1)); } @@ -652,9 +651,11 @@ void MeshDisplay::updateVertexCostsTopic() m_vertexCostsSubscriber.unsubscribe(); delete m_costsSynchronizer; + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + rmw_qos_profile_t qos = rmw_qos_profile_default; qos.depth = 4; - m_vertexCostsSubscriber.subscribe(m_node, m_vertexCostsTopic->getTopicStd(), qos); + m_vertexCostsSubscriber.subscribe(node, m_vertexCostsTopic->getTopicStd(), qos); m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); m_costsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexCosts, this, _1)); } @@ -682,8 +683,9 @@ void MeshDisplay::updateMaterialAndTextureServices() } // Update material and texture service clients - m_materialsClient = m_node->create_client(m_materialServiceName->getStdString()); - m_textureClient = m_node->create_client(m_textureServiceName->getStdString()); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + m_materialsClient = node->create_client(m_materialServiceName->getStdString()); + m_textureClient = node->create_client(m_textureServiceName->getStdString()); if(m_materialsClient->wait_for_service(std::chrono::seconds(5))) { @@ -720,7 +722,8 @@ void MeshDisplay::updateVertexColorService() } // Update vertex color service client - m_vertexColorClient = m_node->create_client(m_vertexColorServiceName->getStdString()); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + m_vertexColorClient = node->create_client(m_vertexColorServiceName->getStdString()); if(m_vertexColorClient->wait_for_service(std::chrono::seconds(5))) { setStatus(rviz_common::properties::StatusProperty::Ok, "Services", "Vertex Color Service OK"); @@ -745,7 +748,8 @@ void MeshDisplay::initialServiceCall() auto req_uuids = std::make_shared(); auto fut_uuids = m_uuidClient->async_send_request(req_uuids); - if(rclcpp::spin_until_future_complete(m_node, fut_uuids) == rclcpp::FutureReturnCode::SUCCESS) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_uuids) == rclcpp::FutureReturnCode::SUCCESS) { auto res_uuids = fut_uuids.get(); @@ -762,7 +766,8 @@ void MeshDisplay::initialServiceCall() req_geometry->uuid = uuid; auto fut_geometry = m_geometryClient->async_send_request(req_geometry); - if(rclcpp::spin_until_future_complete(m_node, fut_geometry) == rclcpp::FutureReturnCode::SUCCESS) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_geometry) == rclcpp::FutureReturnCode::SUCCESS) { auto res_geometry = fut_geometry.get(); RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Found geometry for UUID=" << uuid); @@ -891,7 +896,8 @@ void MeshDisplay::requestVertexColors(std::string uuid) auto fut_vertex_colors = m_vertexColorClient->async_send_request(req_vertex_colors); - if(rclcpp::spin_until_future_complete(m_node, fut_vertex_colors) == rclcpp::FutureReturnCode::SUCCESS) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_vertex_colors) == rclcpp::FutureReturnCode::SUCCESS) { auto res_vertex_colors = fut_vertex_colors.get(); RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful vertex colors service call!"); @@ -923,8 +929,8 @@ void MeshDisplay::requestMaterials(std::string uuid) req_materials->uuid = uuid; auto fut_materials = m_materialsClient->async_send_request(req_materials); - - if(rclcpp::spin_until_future_complete(m_node, fut_materials) == rclcpp::FutureReturnCode::SUCCESS) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_materials) == rclcpp::FutureReturnCode::SUCCESS) { auto res_materials = fut_materials.get(); @@ -972,7 +978,8 @@ void MeshDisplay::requestMaterials(std::string uuid) auto fut_texture = m_textureClient->async_send_request(req_texture); - if(rclcpp::spin_until_future_complete(m_node, fut_texture) == rclcpp::FutureReturnCode::SUCCESS) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_texture) == rclcpp::FutureReturnCode::SUCCESS) { auto res_texture = fut_texture.get(); RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful texture service call with index %d!", material.texture_index); From 79edf657be350d158dc68b77d7bd72a29e0c9d89 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 10 Nov 2023 10:25:09 +0100 Subject: [PATCH 24/40] new name. map only can be confused with other map types --- .../CHANGELOG.rst | 4 +- .../CMakeLists.txt | 61 +++++++++--------- .../README.md | 0 .../icons/classes/ClusterLabel.png | Bin .../icons/classes/Map3D.png | Bin .../icons/classes/Mesh.png | Bin .../icons/classes/MeshGoal.png | Bin .../include/rviz_mesh_map_plugin}/CLUtil.hpp | 4 +- .../ClusterLabelDisplay.hpp | 6 +- .../ClusterLabelPanel.hpp | 10 +-- .../ClusterLabelTool.hpp | 26 ++++---- .../ClusterLabelVisual.hpp | 6 +- .../rviz_mesh_map_plugin}/MapDisplay.hpp | 36 +++++------ .../rviz_mesh_map_plugin}/MeshDisplay.hpp | 12 ++-- .../rviz_mesh_map_plugin}/MeshGoalTool.hpp | 6 +- .../rviz_mesh_map_plugin}/MeshPoseTool.hpp | 17 +++-- .../rviz_mesh_map_plugin}/MeshVisual.hpp | 6 +- .../RVizMessageFilter.hpp | 6 +- .../RvizFileProperty.hpp | 0 .../include/rviz_mesh_map_plugin}/Types.hpp | 4 +- .../kernels/cast_rays.cl | 0 .../package.xml | 11 ++-- .../plugins_description.xml | 26 ++++---- .../src/ClusterLabelDisplay.cpp | 14 ++-- .../src/ClusterLabelPanel.cpp | 10 +-- .../src/ClusterLabelTool.cpp | 20 +++--- .../src/ClusterLabelVisual.cpp | 6 +- .../src/MapDisplay.cpp | 29 ++++----- .../src/MeshDisplay.cpp | 54 +++++++--------- .../src/MeshGoalTool.cpp | 6 +- .../src/MeshPoseTool.cpp | 6 +- .../src/MeshVisual.cpp | 46 ++++++------- .../src/RvizFileProperty.cpp | 2 +- 33 files changed, 215 insertions(+), 219 deletions(-) rename {rviz_map_plugin => rviz_mesh_map_plugin}/CHANGELOG.rst (95%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/CMakeLists.txt (77%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/README.md (100%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/icons/classes/ClusterLabel.png (100%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/icons/classes/Map3D.png (100%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/icons/classes/Mesh.png (100%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/icons/classes/MeshGoal.png (100%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/CLUtil.hpp (99%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/ClusterLabelDisplay.hpp (98%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/ClusterLabelPanel.hpp (95%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/ClusterLabelTool.hpp (94%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/ClusterLabelVisual.hpp (97%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/MapDisplay.hpp (89%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/MeshDisplay.hpp (98%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/MeshGoalTool.hpp (96%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/MeshPoseTool.hpp (91%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/MeshVisual.hpp (99%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/RVizMessageFilter.hpp (68%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/RvizFileProperty.hpp (100%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/Types.hpp (98%) rename {rviz_map_plugin/include => rviz_mesh_map_plugin/include/rviz_mesh_map_plugin}/kernels/cast_rays.cl (100%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/package.xml (83%) rename rviz_map_plugin/rviz_plugin.xml => rviz_mesh_map_plugin/plugins_description.xml (56%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/ClusterLabelDisplay.cpp (96%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/ClusterLabelPanel.cpp (94%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/ClusterLabelTool.cpp (97%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/ClusterLabelVisual.cpp (98%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/MapDisplay.cpp (96%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/MeshDisplay.cpp (92%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/MeshGoalTool.cpp (95%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/MeshPoseTool.cpp (98%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/MeshVisual.cpp (92%) rename {rviz_map_plugin => rviz_mesh_map_plugin}/src/RvizFileProperty.cpp (98%) diff --git a/rviz_map_plugin/CHANGELOG.rst b/rviz_mesh_map_plugin/CHANGELOG.rst similarity index 95% rename from rviz_map_plugin/CHANGELOG.rst rename to rviz_mesh_map_plugin/CHANGELOG.rst index f132a26..48e5fd1 100644 --- a/rviz_map_plugin/CHANGELOG.rst +++ b/rviz_mesh_map_plugin/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rviz_map_plugin +Changelog for package rviz_mesh_map_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.1.0 (2021-10-27) @@ -21,7 +21,7 @@ Changelog for package rviz_map_plugin * MeshDisplay now woring with MeshMap * added properties to MeshDisplay * added Map3D file dialog property -* added textured mesh display for mesh msgs visualization to rviz_map_plugin +* added textured mesh display for mesh msgs visualization to rviz_mesh_map_plugin * combined mesh-plugin and map-plugin texture mesh visuals * fixed mpi error * resolved catkin lint problems diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_mesh_map_plugin/CMakeLists.txt similarity index 77% rename from rviz_map_plugin/CMakeLists.txt rename to rviz_mesh_map_plugin/CMakeLists.txt index a9499e6..106d086 100644 --- a/rviz_map_plugin/CMakeLists.txt +++ b/rviz_mesh_map_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(rviz_map_plugin) +project(rviz_mesh_map_plugin) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -24,25 +24,20 @@ find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) find_package(message_filters REQUIRED) - find_package(Boost REQUIRED COMPONENTS system) find_package(HDF5 REQUIRED COMPONENTS C CXX HL) -find_package(OpenCL 2) - +find_package(assimp REQUIRED) -find_package(assimp) +# This needs to be optional for non CL devices +find_package(OpenCL 2) -if(assimp_FOUND) - include_directories(${ASSIMP_INCLUDE_DIRS}) - add_definitions(-DWITH_ASSIMP) -endif(assimp_FOUND) +include_directories(${ASSIMP_INCLUDE_DIRS}) ## This setting causes Qt's "MOC" generation to happen automatically. # set(CMAKE_AUTOMOC ON) find_package(Qt5 COMPONENTS Core Widgets) add_definitions(-DQT_NO_KEYWORDS) - include_directories( include ${catkin_INCLUDE_DIRS} @@ -79,13 +74,13 @@ set(MOC_HEADER_FILES # include/MeshGoalTool.hpp ) -foreach(header "${RVIZ_MAP_PLUGIN_MOC_FILES}") - qt5_wrap_cpp(RVIZ_MAP_PLUGIN_MOC_FILES "${header}") +foreach(header "${RVIZ_MESH_MAP_PLUGIN_MOC_FILES}") + qt5_wrap_cpp(RVIZ_MESH_MAP_PLUGIN_MOC_FILES "${header}") endforeach() add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} - ${RVIZ_MAP_PLUGIN_MOC_FILES} + ${RVIZ_MESH_MAP_PLUGIN_MOC_FILES} ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -95,37 +90,45 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} PUBLIC - ${geometry_msgs_TARGETS} - rclcpp::rclcpp - rviz_common::rviz_common rviz_ogre_vendor::OgreMain rviz_ogre_vendor::OgreOverlay - rviz_rendering::rviz_rendering - rviz_default_plugins::rviz_default_plugins - ${sensor_msgs_TARGETS} - tf2::tf2 - ${tf2_geometry_msgs_TARGETS} - tf2_ros::tf2_ros - rmw::rmw - ${message_filters_TARGETS} - ${mesh_msgs_TARGETS} ) target_link_libraries(${PROJECT_NAME} PRIVATE - resource_retriever::resource_retriever ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} ) +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_MESH_MAP_PLUGIN_BUILDING_LIBRARY")# + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies(${PROJECT_NAME} + PUBLIC + rclcpp + rmw + rviz_common + rviz_rendering + std_msgs + hdf5_map_io + mesh_msgs + resource_retriever + tf2_ros + message_filters +) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib/${PROJECT_NAME} LIBRARY DESTINATION lib/${PROJECT_NAME} RUNTIME DESTINATION bin/${PROJECT_NAME} ) -install(FILES - rviz_plugin.xml - DESTINATION share/${PROJECT_NAME}) + + + +# install(FILES +# plugins_description.xml +# DESTINATION share/${PROJECT_NAME}) + install(DIRECTORY icons/ DESTINATION share/${PROJECT_NAME}/icons) diff --git a/rviz_map_plugin/README.md b/rviz_mesh_map_plugin/README.md similarity index 100% rename from rviz_map_plugin/README.md rename to rviz_mesh_map_plugin/README.md diff --git a/rviz_map_plugin/icons/classes/ClusterLabel.png b/rviz_mesh_map_plugin/icons/classes/ClusterLabel.png similarity index 100% rename from rviz_map_plugin/icons/classes/ClusterLabel.png rename to rviz_mesh_map_plugin/icons/classes/ClusterLabel.png diff --git a/rviz_map_plugin/icons/classes/Map3D.png b/rviz_mesh_map_plugin/icons/classes/Map3D.png similarity index 100% rename from rviz_map_plugin/icons/classes/Map3D.png rename to rviz_mesh_map_plugin/icons/classes/Map3D.png diff --git a/rviz_map_plugin/icons/classes/Mesh.png b/rviz_mesh_map_plugin/icons/classes/Mesh.png similarity index 100% rename from rviz_map_plugin/icons/classes/Mesh.png rename to rviz_mesh_map_plugin/icons/classes/Mesh.png diff --git a/rviz_map_plugin/icons/classes/MeshGoal.png b/rviz_mesh_map_plugin/icons/classes/MeshGoal.png similarity index 100% rename from rviz_map_plugin/icons/classes/MeshGoal.png rename to rviz_mesh_map_plugin/icons/classes/MeshGoal.png diff --git a/rviz_map_plugin/include/CLUtil.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/CLUtil.hpp similarity index 99% rename from rviz_map_plugin/include/CLUtil.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/CLUtil.hpp index fd366e7..6bdcab6 100644 --- a/rviz_map_plugin/include/CLUtil.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/CLUtil.hpp @@ -50,7 +50,7 @@ #include -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { /** * @class CLUtil @@ -471,6 +471,6 @@ class CLUtil }; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelDisplay.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelDisplay.hpp similarity index 98% rename from rviz_map_plugin/include/ClusterLabelDisplay.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelDisplay.hpp index 2778956..a36daac 100644 --- a/rviz_map_plugin/include/ClusterLabelDisplay.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelDisplay.hpp @@ -49,7 +49,7 @@ #ifndef CLUSTER_LABEL_DISPLAY_HPP #define CLUSTER_LABEL_DISPLAY_HPP -#include +#include #include #include @@ -117,7 +117,7 @@ class StringProperty; } // End namespace rviz -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { using std::map; using std::shared_ptr; @@ -292,6 +292,6 @@ private Q_SLOTS: }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif diff --git a/rviz_map_plugin/include/ClusterLabelPanel.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelPanel.hpp similarity index 95% rename from rviz_map_plugin/include/ClusterLabelPanel.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelPanel.hpp index 6924ba2..be41c59 100644 --- a/rviz_map_plugin/include/ClusterLabelPanel.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelPanel.hpp @@ -44,17 +44,17 @@ * * Kristin Schmidt * Jan Philipp Vogtherr + * Alexander Mock */ #ifndef CLUSTER_LABEL_PANEL_HPP #define CLUSTER_LABEL_PANEL_HPP -#include -// #include +#include #include #include #include -#include +#include #include // Forward declarations @@ -66,7 +66,7 @@ namespace rviz class Tool; } -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { /** * @class ClusterLabelPanel @@ -144,6 +144,6 @@ public Q_SLOTS: // ros::NodeHandle m_nodeHandle; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelTool.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelTool.hpp similarity index 94% rename from rviz_map_plugin/include/ClusterLabelTool.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelTool.hpp index fb4508d..0acc9cc 100644 --- a/rviz_map_plugin/include/ClusterLabelTool.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelTool.hpp @@ -53,7 +53,7 @@ #define CL_HPP_MINIMUM_OPENCL_VERSION 110 #define CL_HPP_ENABLE_EXCEPTIONS -#include +#include #include @@ -70,7 +70,7 @@ #include #include -// #include + #include #include #include @@ -88,16 +88,16 @@ #ifndef Q_MOC_RUN #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -#endif +#endif // Q_MOC_RUN namespace rviz { @@ -112,7 +112,7 @@ namespace Ogre class Vector3; } // namespace Ogre -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { // Forward declarations class ClusterLabelDisplay; @@ -259,6 +259,6 @@ public Q_SLOTS: cl::Kernel m_clKernelBox; cl::Kernel m_clKernelDirAndDist; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelVisual.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelVisual.hpp similarity index 97% rename from rviz_map_plugin/include/ClusterLabelVisual.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelVisual.hpp index 9af4778..54886a0 100644 --- a/rviz_map_plugin/include/ClusterLabelVisual.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelVisual.hpp @@ -48,7 +48,7 @@ #ifndef CLUSTER_LABEL_VISUAL_HPP #define CLUSTER_LABEL_VISUAL_HPP -#include +#include // #include @@ -71,7 +71,7 @@ class SceneNode; class Mesh; } // End namespace Ogre -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { /** * @class ClusterLabelVisual @@ -172,6 +172,6 @@ class ClusterLabelVisual std::vector m_faces; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/MapDisplay.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MapDisplay.hpp similarity index 89% rename from rviz_map_plugin/include/MapDisplay.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MapDisplay.hpp index c06deb6..dcb92a2 100644 --- a/rviz_map_plugin/include/MapDisplay.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MapDisplay.hpp @@ -49,8 +49,8 @@ #ifndef MAP_DISPLAY_HPP #define MAP_DISPLAY_HPP -#include -#include "RvizFileProperty.hpp" +#include +#include #include #include @@ -69,8 +69,6 @@ #include #include -// #include -// #include #include #include #include @@ -98,20 +96,20 @@ #ifndef Q_MOC_RUN #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif -#include -#include +#include +#include namespace rviz { @@ -125,7 +123,7 @@ class StringProperty; } // End namespace rviz -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { using std::shared_ptr; using std::string; @@ -222,9 +220,9 @@ private Q_SLOTS: std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) - rviz_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; + rviz_mesh_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; /// Subdisplay: MeshDisplay (for showing the mesh) - rviz_map_plugin::MeshDisplay* m_meshDisplay; + rviz_mesh_map_plugin::MeshDisplay* m_meshDisplay; /** * @brief Create a RViz display from it's unique class_id @@ -234,6 +232,6 @@ private Q_SLOTS: rviz_common::Display* createDisplay(const QString& class_id); }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshDisplay.hpp similarity index 98% rename from rviz_map_plugin/include/MeshDisplay.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshDisplay.hpp index e270033..278dd95 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshDisplay.hpp @@ -54,8 +54,8 @@ #ifndef MESH_DISPLAY_HPP #define MESH_DISPLAY_HPP -#include -#include +#include +#include #include #include @@ -73,8 +73,6 @@ #include #include -// #include -// #include #include #include @@ -87,7 +85,7 @@ #include #include -#include +#include #include @@ -123,7 +121,7 @@ class StringProperty; } // end namespace properties } // end namespace rviz_common -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { using std::shared_ptr; using std::string; @@ -497,6 +495,6 @@ private Q_SLOTS: std::map> m_costCache; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin #endif diff --git a/rviz_map_plugin/include/MeshGoalTool.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshGoalTool.hpp similarity index 96% rename from rviz_map_plugin/include/MeshGoalTool.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshGoalTool.hpp index cc96cc0..23dcc14 100644 --- a/rviz_map_plugin/include/MeshGoalTool.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshGoalTool.hpp @@ -45,7 +45,7 @@ #ifndef MESH_GOAL_TOOL_HPP #define MESH_GOAL_TOOL_HPP -#include "MeshPoseTool.hpp" +#include #include #include #include @@ -55,7 +55,7 @@ #include #endif -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { /** * @class MeshGoalTool @@ -100,6 +100,6 @@ private Q_SLOTS: ros::NodeHandle nh_; }; -} /* namespace rviz_map_plugin */ +} /* namespace rviz_mesh_map_plugin */ #endif diff --git a/rviz_map_plugin/include/MeshPoseTool.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshPoseTool.hpp similarity index 91% rename from rviz_map_plugin/include/MeshPoseTool.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshPoseTool.hpp index dc3eba1..60d67f9 100644 --- a/rviz_map_plugin/include/MeshPoseTool.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshPoseTool.hpp @@ -45,17 +45,16 @@ #ifndef MESH_POSE_TOOL_HPP #define MESH_POSE_TOOL_HPP -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include -#include +#include +#include -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { class MeshPoseTool : public rviz_common::Tool { @@ -92,6 +91,6 @@ class MeshPoseTool : public rviz_common::Tool Ogre::Vector3 ori_; }; -} /* namespace rviz_map_plugin */ +} /* namespace rviz_mesh_map_plugin */ #endif diff --git a/rviz_map_plugin/include/MeshVisual.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshVisual.hpp similarity index 99% rename from rviz_map_plugin/include/MeshVisual.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshVisual.hpp index f4f7f62..b24ef97 100644 --- a/rviz_map_plugin/include/MeshVisual.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshVisual.hpp @@ -76,7 +76,7 @@ #include #include -#include +#include #include namespace Ogre @@ -88,7 +88,7 @@ class Entity; } // End namespace Ogre -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { /** * @brief Class to display mesh data in the main panel of rviz. @@ -379,6 +379,6 @@ class MeshVisual /// raw normals std::vector m_geometryNormals; }; -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin #endif diff --git a/rviz_map_plugin/include/RVizMessageFilter.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RVizMessageFilter.hpp similarity index 68% rename from rviz_map_plugin/include/RVizMessageFilter.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RVizMessageFilter.hpp index b03656a..e315c90 100644 --- a/rviz_map_plugin/include/RVizMessageFilter.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RVizMessageFilter.hpp @@ -1,5 +1,5 @@ -#ifndef RVIZ_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP -#define RVIZ_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP +#ifndef RVIZ_MESH_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP +#define RVIZ_MESH_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP #include #include @@ -15,4 +15,4 @@ using RVizMessageFilterPtr = std::shared_ptr >; } // namespace tf2_ros -#endif // RVIZ_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP \ No newline at end of file +#endif // RVIZ_MESH_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP \ No newline at end of file diff --git a/rviz_map_plugin/include/RvizFileProperty.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RvizFileProperty.hpp similarity index 100% rename from rviz_map_plugin/include/RvizFileProperty.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RvizFileProperty.hpp diff --git a/rviz_map_plugin/include/Types.hpp b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/Types.hpp similarity index 98% rename from rviz_map_plugin/include/Types.hpp rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/Types.hpp index 82cf603..504067c 100644 --- a/rviz_map_plugin/include/Types.hpp +++ b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/Types.hpp @@ -53,7 +53,7 @@ #include #include -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { using boost::optional; using std::array; @@ -176,4 +176,4 @@ struct Material vector faceIndices; }; -} // namespace rviz_map_plugin +} // namespace rviz_mesh_map_plugin diff --git a/rviz_map_plugin/include/kernels/cast_rays.cl b/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/kernels/cast_rays.cl similarity index 100% rename from rviz_map_plugin/include/kernels/cast_rays.cl rename to rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/kernels/cast_rays.cl diff --git a/rviz_map_plugin/package.xml b/rviz_mesh_map_plugin/package.xml similarity index 83% rename from rviz_map_plugin/package.xml rename to rviz_mesh_map_plugin/package.xml index 95e801b..e8983b8 100644 --- a/rviz_map_plugin/package.xml +++ b/rviz_mesh_map_plugin/package.xml @@ -1,7 +1,7 @@ - rviz_map_plugin + rviz_mesh_map_plugin RViz display types and tools for the mesh_msgs package. @@ -14,11 +14,13 @@ catkin BSD-3 - https://github.com/uos/mesh_tools/tree/master/rviz_map_plugin + https://github.com/uos/mesh_tools/tree/master/rviz_mesh_map_plugin ament_cmake - qtbase5-dev + qtbase5-dev + rviz_ogre_vendor + rclcpp rviz_common rviz_rendering @@ -30,12 +32,13 @@ message_filters ocl-icd-opencl-dev opencl-headers + libqt5-core libqt5-gui libqt5-widgets + rviz_ogre_vendor ament_cmake - diff --git a/rviz_map_plugin/rviz_plugin.xml b/rviz_mesh_map_plugin/plugins_description.xml similarity index 56% rename from rviz_map_plugin/rviz_plugin.xml rename to rviz_mesh_map_plugin/plugins_description.xml index 8da02e8..5c13ceb 100644 --- a/rviz_map_plugin/rviz_plugin.xml +++ b/rviz_mesh_map_plugin/plugins_description.xml @@ -1,41 +1,41 @@ - - + A panel widget allowing creation of cluster labels. - A tool allowing selection of clusters. - Displays labeled clusters of a map. (Don't use without Map3D) - Displays a map with labeled clusters. - Displays a mesh. - Select a goal on a mesh. diff --git a/rviz_map_plugin/src/ClusterLabelDisplay.cpp b/rviz_mesh_map_plugin/src/ClusterLabelDisplay.cpp similarity index 96% rename from rviz_map_plugin/src/ClusterLabelDisplay.cpp rename to rviz_mesh_map_plugin/src/ClusterLabelDisplay.cpp index de7fc95..5383127 100644 --- a/rviz_map_plugin/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_map_plugin/src/ClusterLabelDisplay.cpp @@ -46,9 +46,9 @@ * Jan Philipp Vogtherr */ -#include -#include -#include +#include +#include +#include #include #include @@ -59,7 +59,7 @@ #include "rclcpp/rclcpp.hpp" -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { Ogre::ColourValue getRainbowColor(float value) { @@ -335,7 +335,7 @@ void ClusterLabelDisplay::initializeLabelTool() if (!foundTool) { - m_tool = static_cast(context_->getToolManager()->addTool("rviz_map_plugin/ClusterLabel")); + m_tool = static_cast(context_->getToolManager()->addTool("rviz_mesh_map_plugin/ClusterLabel")); } } @@ -351,7 +351,7 @@ void ClusterLabelDisplay::addLabel(std::string label, std::vector face Q_EMIT signalAddLabel(Cluster(label, faces)); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::ClusterLabelDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/ClusterLabelPanel.cpp b/rviz_mesh_map_plugin/src/ClusterLabelPanel.cpp similarity index 94% rename from rviz_map_plugin/src/ClusterLabelPanel.cpp rename to rviz_mesh_map_plugin/src/ClusterLabelPanel.cpp index 4c5aabb..68c6c85 100644 --- a/rviz_map_plugin/src/ClusterLabelPanel.cpp +++ b/rviz_mesh_map_plugin/src/ClusterLabelPanel.cpp @@ -46,7 +46,7 @@ * Jan Philipp Vogtherr */ -#include +#include #include #include @@ -58,7 +58,7 @@ #include -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz_common::Panel(parent) { @@ -102,7 +102,7 @@ void ClusterLabelPanel::onInitialize() if (!foundTool) { - m_tool = static_cast(vis_manager_->getToolManager()->addTool("rviz_map_plugin/ClusterLabel")); + m_tool = static_cast(vis_manager_->getToolManager()->addTool("rviz_mesh_map_plugin/ClusterLabel")); } } @@ -150,7 +150,7 @@ void ClusterLabelPanel::load(const rviz_common::Config& config) } } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelPanel, rviz_common::Panel) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::ClusterLabelPanel, rviz_common::Panel) diff --git a/rviz_map_plugin/src/ClusterLabelTool.cpp b/rviz_mesh_map_plugin/src/ClusterLabelTool.cpp similarity index 97% rename from rviz_map_plugin/src/ClusterLabelTool.cpp rename to rviz_mesh_map_plugin/src/ClusterLabelTool.cpp index 15456a2..c9c24df 100644 --- a/rviz_map_plugin/src/ClusterLabelTool.cpp +++ b/rviz_mesh_map_plugin/src/ClusterLabelTool.cpp @@ -45,10 +45,10 @@ * */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -59,15 +59,15 @@ #include -#include +#include #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelTool, rviz_common::Tool) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::ClusterLabelTool, rviz_common::Tool) using std::ifstream; using std::stringstream; -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" @@ -195,10 +195,10 @@ void ClusterLabelTool::onInitialize() RCLCPP_DEBUG(" "); // Read kernel file - ifstream in(ros::package::getPath("rviz_map_plugin") + CL_RAY_CAST_KERNEL_FILE); + ifstream in(ros::package::getPath("rviz_mesh_map_plugin") + CL_RAY_CAST_KERNEL_FILE); std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - RCLCPP_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_map_plugin").c_str(), CL_RAY_CAST_KERNEL_FILE); + RCLCPP_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_mesh_map_plugin").c_str(), CL_RAY_CAST_KERNEL_FILE); m_clProgramSources = cl::Program::Sources(1, { cast_rays_kernel.c_str(), cast_rays_kernel.length() }); @@ -734,4 +734,4 @@ void ClusterLabelTool::resetVisual() m_visual.reset(); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin diff --git a/rviz_map_plugin/src/ClusterLabelVisual.cpp b/rviz_mesh_map_plugin/src/ClusterLabelVisual.cpp similarity index 98% rename from rviz_map_plugin/src/ClusterLabelVisual.cpp rename to rviz_mesh_map_plugin/src/ClusterLabelVisual.cpp index 327e28b..488cc72 100644 --- a/rviz_map_plugin/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_map_plugin/src/ClusterLabelVisual.cpp @@ -45,7 +45,7 @@ * Kristin Schmidt */ -#include +#include #include @@ -57,7 +57,7 @@ #include #include -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId) : m_displayContext(context), m_labelId(labelId) @@ -271,4 +271,4 @@ void ClusterLabelVisual::initMaterial() m_material->setDepthWriteEnabled(false); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_mesh_map_plugin/src/MapDisplay.cpp similarity index 96% rename from rviz_map_plugin/src/MapDisplay.cpp rename to rviz_mesh_map_plugin/src/MapDisplay.cpp index 0c1b842..2c7b75e 100644 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ b/rviz_mesh_map_plugin/src/MapDisplay.cpp @@ -46,9 +46,9 @@ * Jan Philipp Vogtherr */ -#include -#include -#include +#include +#include +#include #include #include @@ -60,13 +60,12 @@ #include #include -#if defined(WITH_ASSIMP) - #include - #include - #include -#endif +#include +#include +#include -namespace rviz_map_plugin + +namespace rviz_mesh_map_plugin { MapDisplay::MapDisplay() { @@ -109,7 +108,7 @@ void MapDisplay::onInitialize() { std::string name = this->getName().toStdString(); - Display* display = createDisplay("rviz_map_plugin/ClusterLabel"); + Display* display = createDisplay("rviz_mesh_map_plugin/ClusterLabel"); m_nh = std::make_shared("~"); m_nh_p = std::make_shared("~"); @@ -121,7 +120,7 @@ void MapDisplay::onInitialize() addChild(m_clusterLabelDisplay); m_clusterLabelDisplay->initialize(context_); - Display* meshDisplay = createDisplay("rviz_map_plugin/Mesh"); + Display* meshDisplay = createDisplay("rviz_mesh_map_plugin/Mesh"); m_meshDisplay = static_cast(meshDisplay); addChild(m_meshDisplay); @@ -159,7 +158,7 @@ void MapDisplay::load(const rviz_common::Config& config) { // Override with ros params std::stringstream ss; - ss << "rviz_map_plugin/" << name; + ss << "rviz_mesh_map_plugin/" << name; std::string mesh_file; if(m_nh_p->getParam(ss.str(), mesh_file)) @@ -220,7 +219,7 @@ bool MapDisplay::loadData() std::string name = this->getName().toStdString(); // std::stringstream ss; - // ss << "rviz_map_plugin/" << name; + // ss << "rviz_mesh_map_plugin/" << name; // std::string mesh_file; // if(m_nh_p->getParam(ss.str(), mesh_file)) @@ -493,7 +492,7 @@ void MapDisplay::saveLabel(Cluster cluster) } } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MapDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::MapDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_mesh_map_plugin/src/MeshDisplay.cpp similarity index 92% rename from rviz_map_plugin/src/MeshDisplay.cpp rename to rviz_mesh_map_plugin/src/MeshDisplay.cpp index 4269056..97d7cd5 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_mesh_map_plugin/src/MeshDisplay.cpp @@ -51,7 +51,7 @@ * Malte kleine Piening */ -#include +#include #include #include @@ -86,7 +86,7 @@ using namespace std::chrono_literals; using std::placeholders::_1; using std::placeholders::_2; -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { MeshDisplay::MeshDisplay() : rviz_common::Display() @@ -256,16 +256,12 @@ void MeshDisplay::onInitialize() // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - m_vertexColorClient = node->create_client(m_vertexColorServiceName->getStdString()); m_materialsClient = node->create_client(m_vertexColorServiceName->getStdString()); m_textureClient = node->create_client(m_vertexColorServiceName->getStdString()); m_uuidClient = node->create_client("get_uuid"); m_geometryClient = node->create_client("get_geometry"); - - - updateMesh(); updateWireframe(); updateNormals(); @@ -464,7 +460,7 @@ void MeshDisplay::updateBufferSize() void MeshDisplay::updateMesh() { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Mesh Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Mesh Display: Update"); bool showFaces = false; bool showTextures = false; @@ -549,7 +545,7 @@ void MeshDisplay::updateMesh() std::shared_ptr visual = getLatestVisual(); if (!visual) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); return; } @@ -700,7 +696,7 @@ void MeshDisplay::updateMaterialAndTextureServices() setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); } } else { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); } } @@ -759,7 +755,7 @@ void MeshDisplay::initialServiceCall() { std::string uuid = uuids[0]; - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Initial data available for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Initial data available for UUID=" << uuid); // mesh_msgs::srv::GetGeometry srv_geometry; auto req_geometry = std::make_shared(); @@ -770,15 +766,15 @@ void MeshDisplay::initialServiceCall() if(rclcpp::spin_until_future_complete(node, fut_geometry) == rclcpp::FutureReturnCode::SUCCESS) { auto res_geometry = fut_geometry.get(); - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Found geometry for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Found geometry for UUID=" << uuid); processMessage(res_geometry->mesh_geometry_stamped); } else { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); } } } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "No initial data available, waiting for callback to trigger ..."); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "No initial data available, waiting for callback to trigger ..."); } } @@ -795,14 +791,14 @@ void MeshDisplay::processMessage(const mesh_msgs::msg::MeshGeometryStamped& mesh if (!context_->getFrameManager()->getTransform(meshMsg.header.frame_id, meshMsg.header.stamp, position, orientation)) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Error transforming from frame '%s' to frame '%s'", meshMsg.header.frame_id.c_str(), + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Error transforming from frame '%s' to frame '%s'", meshMsg.header.frame_id.c_str(), qPrintable(rviz_common::Display::fixed_frame_)); return; } if (!m_lastUuid.empty() && meshMsg.uuid.compare(m_lastUuid) != 0) { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received geometry with new UUID!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received geometry with new UUID!"); m_costCache.clear(); m_selectVertexCostMap->clearOptions(); m_selectVertexCostMap->addOption("-- None --", 0); @@ -857,7 +853,7 @@ void MeshDisplay::incomingVertexColors( { if (colorsStamped->uuid.compare(m_lastUuid) != 0) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex colors, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received vertex colors, but UUIDs dont match!"); return; } @@ -876,7 +872,7 @@ void MeshDisplay::incomingVertexCosts( { if (costsStamped->uuid.compare(m_lastUuid) != 0) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Received vertex costs, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received vertex costs, but UUIDs dont match!"); return; } @@ -900,7 +896,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) if(rclcpp::spin_until_future_complete(node, fut_vertex_colors) == rclcpp::FutureReturnCode::SUCCESS) { auto res_vertex_colors = fut_vertex_colors.get(); - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Successful vertex colors service call!"); std::vector vertexColors; for (const std_msgs::msg::ColorRGBA c : res_vertex_colors->mesh_vertex_colors_stamped.mesh_vertex_colors.vertex_colors) @@ -913,7 +909,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Failed vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed vertex colors service call!"); } } @@ -934,7 +930,7 @@ void MeshDisplay::requestMaterials(std::string uuid) { auto res_materials = fut_materials.get(); - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Successful materials service call!"); const mesh_msgs::msg::MeshMaterialsStamped& meshMaterialsStamped = res_materials->mesh_materials_stamped; @@ -982,7 +978,7 @@ void MeshDisplay::requestMaterials(std::string uuid) if(rclcpp::spin_until_future_complete(node, fut_texture) == rclcpp::FutureReturnCode::SUCCESS) { auto res_texture = fut_texture.get(); - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Successful texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Successful texture service call with index %d!", material.texture_index); const mesh_msgs::msg::MeshTexture& textureMsg = res_texture->texture; @@ -997,31 +993,31 @@ void MeshDisplay::requestMaterials(std::string uuid) } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Failed texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed texture service call with index %d!", material.texture_index); } } } } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Failed materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed materials service call!"); } } void MeshDisplay::cacheVertexCosts(std::string layer, const std::vector& costs) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "Cache vertex cost map '" << layer << "' for UUID "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Cache vertex cost map '" << layer << "' for UUID "); // insert into cache auto it = m_costCache.find(layer); if (it != m_costCache.end()) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "The cost layer \"" << layer << "\" has been updated."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "The cost layer \"" << layer << "\" has been updated."); m_costCache.erase(layer); } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_map_plugin"), "The cost layer \"" << layer << "\" has been added."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "The cost layer \"" << layer << "\" has been added."); m_selectVertexCostMap->addOptionStd(layer, m_selectVertexCostMap->numChildren()); } @@ -1056,7 +1052,7 @@ std::shared_ptr MeshDisplay::addNewVisual() return m_visuals.back(); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_map_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MeshDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::MeshDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/MeshGoalTool.cpp b/rviz_mesh_map_plugin/src/MeshGoalTool.cpp similarity index 95% rename from rviz_map_plugin/src/MeshGoalTool.cpp rename to rviz_mesh_map_plugin/src/MeshGoalTool.cpp index 75f65ab..ba38342 100644 --- a/rviz_map_plugin/src/MeshGoalTool.cpp +++ b/rviz_mesh_map_plugin/src/MeshGoalTool.cpp @@ -43,12 +43,12 @@ */ -#include "MeshGoalTool.hpp" +#include "rviz_mesh_map_plugin/MeshGoalTool.hpp" #include -PLUGINLIB_EXPORT_CLASS( rviz_map_plugin::MeshGoalTool, rviz_common::Tool ) +PLUGINLIB_EXPORT_CLASS( rviz_mesh_map_plugin::MeshGoalTool, rviz_common::Tool ) -namespace rviz_map_plugin{ +namespace rviz_mesh_map_plugin{ MeshGoalTool::MeshGoalTool() { shortcut_key_ = 'm'; diff --git a/rviz_map_plugin/src/MeshPoseTool.cpp b/rviz_mesh_map_plugin/src/MeshPoseTool.cpp similarity index 98% rename from rviz_map_plugin/src/MeshPoseTool.cpp rename to rviz_mesh_map_plugin/src/MeshPoseTool.cpp index 7981127..0bb4c8c 100644 --- a/rviz_map_plugin/src/MeshPoseTool.cpp +++ b/rviz_mesh_map_plugin/src/MeshPoseTool.cpp @@ -55,9 +55,9 @@ #include #include -#include "MeshPoseTool.hpp" +#include "rviz_mesh_map_plugin/MeshPoseTool.hpp" -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { @@ -294,4 +294,4 @@ void MeshPoseTool::getRawManualObjectData(const Ogre::ManualObject* mesh, const indexBuffer->unlock(); } -} // namespace rviz_map_plugin +} // namespace rviz_mesh_map_plugin diff --git a/rviz_map_plugin/src/MeshVisual.cpp b/rviz_mesh_map_plugin/src/MeshVisual.cpp similarity index 92% rename from rviz_map_plugin/src/MeshVisual.cpp rename to rviz_mesh_map_plugin/src/MeshVisual.cpp index 29057ee..1acafb1 100644 --- a/rviz_map_plugin/src/MeshVisual.cpp +++ b/rviz_mesh_map_plugin/src/MeshVisual.cpp @@ -50,7 +50,7 @@ * Jan Philipp Vogtherr */ -#include "MeshVisual.hpp" +#include "rviz_mesh_map_plugin/MeshVisual.hpp" #include #include @@ -64,7 +64,7 @@ #include "rclcpp/rclcpp.hpp" -namespace rviz_map_plugin +namespace rviz_mesh_map_plugin { Ogre::ColourValue getRainbowColor1(float value) { @@ -107,7 +107,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s , m_texture_coords_enabled(false) , m_normalsScalingFactor(1) { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); // get or create the scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); @@ -161,7 +161,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s MeshVisual::~MeshVisual() { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); reset(); @@ -192,7 +192,7 @@ MeshVisual::~MeshVisual() void MeshVisual::reset() { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); std::stringstream sstm; @@ -557,7 +557,7 @@ void MeshVisual::enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const float range = maxCost - minCost; if (range <= 0) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_map_plugin"), "Illegal vertex cost limits!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Illegal vertex cost limits!"); return; } @@ -654,7 +654,7 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector // if the image was only default constructed, in which case its width will be 0 if (m_images.size() < textureIndex + 1 || m_images[textureIndex].getWidth() == 0) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_map_plugin"), "Texture with index %u not loaded yet", textureIndex); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_map_plugin"), "Texture with index %u not loaded yet", textureIndex); } else { @@ -783,7 +783,7 @@ bool MeshVisual::setGeometry(const Geometry& mesh) // check if there are enough vertices given if (mesh.vertices.size() < 3) { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not enough vertices, can't create mesh!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not enough vertices, can't create mesh!"); return false; } @@ -807,12 +807,12 @@ bool MeshVisual::setNormals(const vector& normals) // check if there are vertex normals for each vertex if (normals.size() == m_geometry.vertices.size()) { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex normals.", normals.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex normals.", normals.size()); m_vertex_normals_enabled = true; } else if (normals.size() > 0) { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex normals as vertices, ignoring vertex normals!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex normals as vertices, ignoring vertex normals!"); return false; } @@ -836,12 +836,12 @@ bool MeshVisual::setVertexColors(const vector& vertexColors) // check if there are vertex colors for each vertex if (vertexColors.size() == m_geometry.vertices.size()) { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex colors.", vertexColors.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex colors.", vertexColors.size()); m_vertex_colors_enabled = true; } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex colors as vertices, ignoring the vertex colors!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex colors as vertices, ignoring the vertex colors!"); return false; } @@ -867,12 +867,12 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -896,12 +896,12 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -917,12 +917,12 @@ bool MeshVisual::setMaterials(const vector& materials, const vector= 0) { - RCLCPP_INFO(rclcpp::get_logger("rviz_map_plugin"), "Received %lu materials.", materials.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu materials.", materials.size()); m_materials_enabled = true; // enable materials } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received zero materials, ignoring materials!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received zero materials, ignoring materials!"); return false; } @@ -930,13 +930,13 @@ bool MeshVisual::setMaterials(const vector& materials, const vector 0) { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Received not as much texture coords as vertices, ignoring texture coords!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much texture coords as vertices, ignoring texture coords!"); } enteringTexturedTriangleMesh(m_geometry, materials, texCoords); @@ -965,7 +965,7 @@ bool MeshVisual::addTexture(Texture& texture, uint32_t textureIndex) } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Can't load image into texture material, material does not exist!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Can't load image into texture material, material does not exist!"); return false; } } @@ -981,7 +981,7 @@ Ogre::PixelFormat MeshVisual::getOgrePixelFormatFromRosString(std::string encodi return Ogre::PF_BYTE_RGB; } - RCLCPP_WARN(rclcpp::get_logger("rviz_map_plugin"), "Unknown texture encoding! Using Ogre::PF_UNKNOWN"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Unknown texture encoding! Using Ogre::PF_UNKNOWN"); return Ogre::PF_UNKNOWN; } @@ -1035,4 +1035,4 @@ void MeshVisual::setFrameOrientation(const Ogre::Quaternion& orientation) m_sceneNode->setOrientation(orientation); } -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_map_plugin diff --git a/rviz_map_plugin/src/RvizFileProperty.cpp b/rviz_mesh_map_plugin/src/RvizFileProperty.cpp similarity index 98% rename from rviz_map_plugin/src/RvizFileProperty.cpp rename to rviz_mesh_map_plugin/src/RvizFileProperty.cpp index 94afdca..654baf9 100644 --- a/rviz_map_plugin/src/RvizFileProperty.cpp +++ b/rviz_mesh_map_plugin/src/RvizFileProperty.cpp @@ -45,7 +45,7 @@ * Malte kleine Piening */ -#include "RvizFileProperty.hpp" +#include "rviz_mesh_map_plugin/RvizFileProperty.hpp" #include From f9c8e1c4198a1589b448e0b1bb868c380ee249ba Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 10 Nov 2023 10:39:46 +0100 Subject: [PATCH 25/40] since there are more plugins than only mesh map I renamed it again --- .../CHANGELOG.rst | 4 +- .../CMakeLists.txt | 53 +++++++++++++----- .../README.md | 0 .../icons/classes/ClusterLabel.png | Bin .../icons/classes/Map3D.png | Bin .../icons/classes/Mesh.png | Bin .../icons/classes/MeshGoal.png | Bin .../rviz_mesh_tools_plugins}/CLUtil.hpp | 4 +- .../ClusterLabelDisplay.hpp | 6 +- .../ClusterLabelPanel.hpp | 8 +-- .../ClusterLabelTool.hpp | 6 +- .../ClusterLabelVisual.hpp | 6 +- .../rviz_mesh_tools_plugins}/MapDisplay.hpp | 16 +++--- .../rviz_mesh_tools_plugins}/MeshDisplay.hpp | 10 ++-- .../rviz_mesh_tools_plugins}/MeshGoalTool.hpp | 6 +- .../rviz_mesh_tools_plugins}/MeshPoseTool.hpp | 4 +- .../rviz_mesh_tools_plugins}/MeshVisual.hpp | 6 +- .../RVizMessageFilter.hpp | 6 +- .../RvizFileProperty.hpp | 0 .../rviz_mesh_tools_plugins}/Types.hpp | 5 +- .../kernels/cast_rays.cl | 0 .../package.xml | 4 +- .../plugins_description.xml | 26 ++++----- .../rviz_mesh_tools_plugins-extras.cmake | 46 +++++++++++++++ .../src/ClusterLabelDisplay.cpp | 14 ++--- .../src/ClusterLabelPanel.cpp | 10 ++-- .../src/ClusterLabelTool.cpp | 18 +++--- .../src/ClusterLabelVisual.cpp | 6 +- .../src/MapDisplay.cpp | 20 +++---- .../src/MeshDisplay.cpp | 50 ++++++++--------- .../src/MeshGoalTool.cpp | 6 +- .../src/MeshPoseTool.cpp | 6 +- .../src/MeshVisual.cpp | 46 +++++++-------- .../src/RvizFileProperty.cpp | 2 +- 34 files changed, 232 insertions(+), 162 deletions(-) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/CHANGELOG.rst (94%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/CMakeLists.txt (72%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/README.md (100%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/icons/classes/ClusterLabel.png (100%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/icons/classes/Map3D.png (100%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/icons/classes/Mesh.png (100%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/icons/classes/MeshGoal.png (100%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/CLUtil.hpp (99%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/ClusterLabelDisplay.hpp (98%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/ClusterLabelPanel.hpp (95%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/ClusterLabelTool.hpp (98%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/ClusterLabelVisual.hpp (97%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/MapDisplay.hpp (93%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/MeshDisplay.hpp (98%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/MeshGoalTool.hpp (95%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/MeshPoseTool.hpp (97%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/MeshVisual.hpp (99%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/RVizMessageFilter.hpp (67%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/RvizFileProperty.hpp (100%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/Types.hpp (97%) rename {rviz_mesh_map_plugin/include/rviz_mesh_map_plugin => rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins}/kernels/cast_rays.cl (100%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/package.xml (92%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/plugins_description.xml (55%) create mode 100644 rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/ClusterLabelDisplay.cpp (96%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/ClusterLabelPanel.cpp (94%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/ClusterLabelTool.cpp (97%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/ClusterLabelVisual.cpp (98%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/MapDisplay.cpp (96%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/MeshDisplay.cpp (92%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/MeshGoalTool.cpp (95%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/MeshPoseTool.cpp (98%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/MeshVisual.cpp (92%) rename {rviz_mesh_map_plugin => rviz_mesh_tools_plugins}/src/RvizFileProperty.cpp (98%) diff --git a/rviz_mesh_map_plugin/CHANGELOG.rst b/rviz_mesh_tools_plugins/CHANGELOG.rst similarity index 94% rename from rviz_mesh_map_plugin/CHANGELOG.rst rename to rviz_mesh_tools_plugins/CHANGELOG.rst index 48e5fd1..89b1da1 100644 --- a/rviz_mesh_map_plugin/CHANGELOG.rst +++ b/rviz_mesh_tools_plugins/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rviz_mesh_map_plugin +Changelog for package rviz_mesh_tools_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.1.0 (2021-10-27) @@ -21,7 +21,7 @@ Changelog for package rviz_mesh_map_plugin * MeshDisplay now woring with MeshMap * added properties to MeshDisplay * added Map3D file dialog property -* added textured mesh display for mesh msgs visualization to rviz_mesh_map_plugin +* added textured mesh display for mesh msgs visualization to rviz_mesh_tools_plugins * combined mesh-plugin and map-plugin texture mesh visuals * fixed mpi error * resolved catkin lint problems diff --git a/rviz_mesh_map_plugin/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt similarity index 72% rename from rviz_mesh_map_plugin/CMakeLists.txt rename to rviz_mesh_tools_plugins/CMakeLists.txt index 106d086..1f8a342 100644 --- a/rviz_mesh_map_plugin/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(rviz_mesh_map_plugin) +project(rviz_mesh_tools_plugins) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -74,13 +74,13 @@ set(MOC_HEADER_FILES # include/MeshGoalTool.hpp ) -foreach(header "${RVIZ_MESH_MAP_PLUGIN_MOC_FILES}") - qt5_wrap_cpp(RVIZ_MESH_MAP_PLUGIN_MOC_FILES "${header}") +foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") + qt5_wrap_cpp(RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES "${header}") endforeach() add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} - ${RVIZ_MESH_MAP_PLUGIN_MOC_FILES} + ${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES} ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -99,7 +99,7 @@ target_link_libraries(${PROJECT_NAME} PRIVATE ${HDF5_HL_LIBRARIES} ) -target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_MESH_MAP_PLUGIN_BUILDING_LIBRARY")# +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_MESH_TOOLS_PLUGINS_BUILDING_LIBRARY")# pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) @@ -117,20 +117,43 @@ ament_target_dependencies(${PROJECT_NAME} message_filters ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION bin/${PROJECT_NAME} +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_export_dependencies( + rclcpp + rmw + rviz_common + rviz_rendering + std_msgs + hdf5_map_io + mesh_msgs + resource_retriever + tf2_ros + message_filters ) +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) -# install(FILES -# plugins_description.xml -# DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY icons/ - DESTINATION share/${PROJECT_NAME}/icons) +install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION share/${PROJECT_NAME}) -ament_package() \ No newline at end of file +ament_package( + CONFIG_EXTRAS "rviz_mesh_tools_plugins-extras.cmake" +) \ No newline at end of file diff --git a/rviz_mesh_map_plugin/README.md b/rviz_mesh_tools_plugins/README.md similarity index 100% rename from rviz_mesh_map_plugin/README.md rename to rviz_mesh_tools_plugins/README.md diff --git a/rviz_mesh_map_plugin/icons/classes/ClusterLabel.png b/rviz_mesh_tools_plugins/icons/classes/ClusterLabel.png similarity index 100% rename from rviz_mesh_map_plugin/icons/classes/ClusterLabel.png rename to rviz_mesh_tools_plugins/icons/classes/ClusterLabel.png diff --git a/rviz_mesh_map_plugin/icons/classes/Map3D.png b/rviz_mesh_tools_plugins/icons/classes/Map3D.png similarity index 100% rename from rviz_mesh_map_plugin/icons/classes/Map3D.png rename to rviz_mesh_tools_plugins/icons/classes/Map3D.png diff --git a/rviz_mesh_map_plugin/icons/classes/Mesh.png b/rviz_mesh_tools_plugins/icons/classes/Mesh.png similarity index 100% rename from rviz_mesh_map_plugin/icons/classes/Mesh.png rename to rviz_mesh_tools_plugins/icons/classes/Mesh.png diff --git a/rviz_mesh_map_plugin/icons/classes/MeshGoal.png b/rviz_mesh_tools_plugins/icons/classes/MeshGoal.png similarity index 100% rename from rviz_mesh_map_plugin/icons/classes/MeshGoal.png rename to rviz_mesh_tools_plugins/icons/classes/MeshGoal.png diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/CLUtil.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp similarity index 99% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/CLUtil.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp index 6bdcab6..100d380 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/CLUtil.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp @@ -50,7 +50,7 @@ #include -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class CLUtil @@ -471,6 +471,6 @@ class CLUtil }; }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp similarity index 98% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelDisplay.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp index a36daac..dd0c422 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp @@ -49,7 +49,7 @@ #ifndef CLUSTER_LABEL_DISPLAY_HPP #define CLUSTER_LABEL_DISPLAY_HPP -#include +#include #include #include @@ -117,7 +117,7 @@ class StringProperty; } // End namespace rviz -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { using std::map; using std::shared_ptr; @@ -292,6 +292,6 @@ private Q_SLOTS: }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelPanel.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp similarity index 95% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelPanel.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp index be41c59..a8197b7 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelPanel.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp @@ -50,11 +50,11 @@ #ifndef CLUSTER_LABEL_PANEL_HPP #define CLUSTER_LABEL_PANEL_HPP -#include +#include #include #include #include -#include +#include #include // Forward declarations @@ -66,7 +66,7 @@ namespace rviz class Tool; } -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class ClusterLabelPanel @@ -144,6 +144,6 @@ public Q_SLOTS: // ros::NodeHandle m_nodeHandle; }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp similarity index 98% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelTool.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 0acc9cc..ef5370e 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -53,7 +53,7 @@ #define CL_HPP_MINIMUM_OPENCL_VERSION 110 #define CL_HPP_ENABLE_EXCEPTIONS -#include +#include #include @@ -112,7 +112,7 @@ namespace Ogre class Vector3; } // namespace Ogre -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { // Forward declarations class ClusterLabelDisplay; @@ -259,6 +259,6 @@ public Q_SLOTS: cl::Kernel m_clKernelBox; cl::Kernel m_clKernelDirAndDist; }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelVisual.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp similarity index 97% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelVisual.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp index 54886a0..a1ce6ec 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/ClusterLabelVisual.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp @@ -48,7 +48,7 @@ #ifndef CLUSTER_LABEL_VISUAL_HPP #define CLUSTER_LABEL_VISUAL_HPP -#include +#include // #include @@ -71,7 +71,7 @@ class SceneNode; class Mesh; } // End namespace Ogre -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class ClusterLabelVisual @@ -172,6 +172,6 @@ class ClusterLabelVisual std::vector m_faces; }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MapDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp similarity index 93% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MapDisplay.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp index dcb92a2..28dc45d 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MapDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp @@ -49,8 +49,8 @@ #ifndef MAP_DISPLAY_HPP #define MAP_DISPLAY_HPP -#include -#include +#include +#include #include #include @@ -108,8 +108,8 @@ #endif -#include -#include +#include +#include namespace rviz { @@ -123,7 +123,7 @@ class StringProperty; } // End namespace rviz -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { using std::shared_ptr; using std::string; @@ -220,9 +220,9 @@ private Q_SLOTS: std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) - rviz_mesh_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; + rviz_mesh_tools_plugins::ClusterLabelDisplay* m_clusterLabelDisplay; /// Subdisplay: MeshDisplay (for showing the mesh) - rviz_mesh_map_plugin::MeshDisplay* m_meshDisplay; + rviz_mesh_tools_plugins::MeshDisplay* m_meshDisplay; /** * @brief Create a RViz display from it's unique class_id @@ -232,6 +232,6 @@ private Q_SLOTS: rviz_common::Display* createDisplay(const QString& class_id); }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp similarity index 98% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshDisplay.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp index 278dd95..7a0a164 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp @@ -54,8 +54,8 @@ #ifndef MESH_DISPLAY_HPP #define MESH_DISPLAY_HPP -#include -#include +#include +#include #include #include @@ -85,7 +85,7 @@ #include #include -#include +#include #include @@ -121,7 +121,7 @@ class StringProperty; } // end namespace properties } // end namespace rviz_common -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { using std::shared_ptr; using std::string; @@ -495,6 +495,6 @@ private Q_SLOTS: std::map> m_costCache; }; -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshGoalTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp similarity index 95% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshGoalTool.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp index 23dcc14..febcbf9 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshGoalTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp @@ -45,7 +45,7 @@ #ifndef MESH_GOAL_TOOL_HPP #define MESH_GOAL_TOOL_HPP -#include +#include #include #include #include @@ -55,7 +55,7 @@ #include #endif -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class MeshGoalTool @@ -100,6 +100,6 @@ private Q_SLOTS: ros::NodeHandle nh_; }; -} /* namespace rviz_mesh_map_plugin */ +} /* namespace rviz_mesh_tools_plugins */ #endif diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshPoseTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp similarity index 97% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshPoseTool.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp index 60d67f9..a41d3f9 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshPoseTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp @@ -54,7 +54,7 @@ #include #include -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { class MeshPoseTool : public rviz_common::Tool { @@ -91,6 +91,6 @@ class MeshPoseTool : public rviz_common::Tool Ogre::Vector3 ori_; }; -} /* namespace rviz_mesh_map_plugin */ +} /* namespace rviz_mesh_tools_plugins */ #endif diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshVisual.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp similarity index 99% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshVisual.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp index b24ef97..c1537e7 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/MeshVisual.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp @@ -76,7 +76,7 @@ #include #include -#include +#include #include namespace Ogre @@ -88,7 +88,7 @@ class Entity; } // End namespace Ogre -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { /** * @brief Class to display mesh data in the main panel of rviz. @@ -379,6 +379,6 @@ class MeshVisual /// raw normals std::vector m_geometryNormals; }; -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RVizMessageFilter.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp similarity index 67% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RVizMessageFilter.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp index e315c90..97d02e6 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RVizMessageFilter.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp @@ -1,5 +1,5 @@ -#ifndef RVIZ_MESH_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP -#define RVIZ_MESH_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP +#ifndef RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP +#define RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP #include #include @@ -15,4 +15,4 @@ using RVizMessageFilterPtr = std::shared_ptr >; } // namespace tf2_ros -#endif // RVIZ_MESH_MAP_PLUGIN_RVIZ_MESSAGE_FILTER_HPP \ No newline at end of file +#endif // RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP \ No newline at end of file diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RvizFileProperty.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp similarity index 100% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/RvizFileProperty.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/Types.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp similarity index 97% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/Types.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp index 504067c..944ffa8 100644 --- a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/Types.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp @@ -44,6 +44,7 @@ * * Kristin Schmidt * Jan Philipp Vogtherr + * Alexander Mock */ #pragma once @@ -53,7 +54,7 @@ #include #include -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { using boost::optional; using std::array; @@ -176,4 +177,4 @@ struct Material vector faceIndices; }; -} // namespace rviz_mesh_map_plugin +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/kernels/cast_rays.cl b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/kernels/cast_rays.cl similarity index 100% rename from rviz_mesh_map_plugin/include/rviz_mesh_map_plugin/kernels/cast_rays.cl rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/kernels/cast_rays.cl diff --git a/rviz_mesh_map_plugin/package.xml b/rviz_mesh_tools_plugins/package.xml similarity index 92% rename from rviz_mesh_map_plugin/package.xml rename to rviz_mesh_tools_plugins/package.xml index e8983b8..c9b54c9 100644 --- a/rviz_mesh_map_plugin/package.xml +++ b/rviz_mesh_tools_plugins/package.xml @@ -1,7 +1,7 @@ - rviz_mesh_map_plugin + rviz_mesh_tools_plugins RViz display types and tools for the mesh_msgs package. @@ -14,7 +14,7 @@ catkin BSD-3 - https://github.com/uos/mesh_tools/tree/master/rviz_mesh_map_plugin + https://github.com/uos/mesh_tools/tree/master/rviz_mesh_tools_plugins ament_cmake diff --git a/rviz_mesh_map_plugin/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml similarity index 55% rename from rviz_mesh_map_plugin/plugins_description.xml rename to rviz_mesh_tools_plugins/plugins_description.xml index 5c13ceb..4e7b0d8 100644 --- a/rviz_mesh_map_plugin/plugins_description.xml +++ b/rviz_mesh_tools_plugins/plugins_description.xml @@ -1,41 +1,41 @@ - - + A panel widget allowing creation of cluster labels. - A tool allowing selection of clusters. - Displays labeled clusters of a map. (Don't use without Map3D) - Displays a map with labeled clusters. - Displays a mesh. - Select a goal on a mesh. diff --git a/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake b/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake new file mode 100644 index 0000000..2724ff1 --- /dev/null +++ b/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake @@ -0,0 +1,46 @@ +# Software License Agreement (BSD License) +# +# Robot Operating System code by the University of Osnabrück +# Copyright (c) 2023, 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. +# +# +# +# rviz_mesh_tools_plugins-extras.cmake +# +# +# authors: +# +# Alexander Mock +# +find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) \ No newline at end of file diff --git a/rviz_mesh_map_plugin/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp similarity index 96% rename from rviz_mesh_map_plugin/src/ClusterLabelDisplay.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 5383127..2bb690e 100644 --- a/rviz_mesh_map_plugin/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -46,9 +46,9 @@ * Jan Philipp Vogtherr */ -#include -#include -#include +#include +#include +#include #include #include @@ -59,7 +59,7 @@ #include "rclcpp/rclcpp.hpp" -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { Ogre::ColourValue getRainbowColor(float value) { @@ -335,7 +335,7 @@ void ClusterLabelDisplay::initializeLabelTool() if (!foundTool) { - m_tool = static_cast(context_->getToolManager()->addTool("rviz_mesh_map_plugin/ClusterLabel")); + m_tool = static_cast(context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); } } @@ -351,7 +351,7 @@ void ClusterLabelDisplay::addLabel(std::string label, std::vector face Q_EMIT signalAddLabel(Cluster(label, faces)); } -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::ClusterLabelDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelDisplay, rviz_common::Display) diff --git a/rviz_mesh_map_plugin/src/ClusterLabelPanel.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp similarity index 94% rename from rviz_mesh_map_plugin/src/ClusterLabelPanel.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp index 68c6c85..0958011 100644 --- a/rviz_mesh_map_plugin/src/ClusterLabelPanel.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp @@ -46,7 +46,7 @@ * Jan Philipp Vogtherr */ -#include +#include #include #include @@ -58,7 +58,7 @@ #include -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz_common::Panel(parent) { @@ -102,7 +102,7 @@ void ClusterLabelPanel::onInitialize() if (!foundTool) { - m_tool = static_cast(vis_manager_->getToolManager()->addTool("rviz_mesh_map_plugin/ClusterLabel")); + m_tool = static_cast(vis_manager_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); } } @@ -150,7 +150,7 @@ void ClusterLabelPanel::load(const rviz_common::Config& config) } } -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::ClusterLabelPanel, rviz_common::Panel) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelPanel, rviz_common::Panel) diff --git a/rviz_mesh_map_plugin/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp similarity index 97% rename from rviz_mesh_map_plugin/src/ClusterLabelTool.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index c9c24df..2d213ff 100644 --- a/rviz_mesh_map_plugin/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -45,10 +45,10 @@ * */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -62,12 +62,12 @@ #include #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::ClusterLabelTool, rviz_common::Tool) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelTool, rviz_common::Tool) using std::ifstream; using std::stringstream; -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" @@ -195,10 +195,10 @@ void ClusterLabelTool::onInitialize() RCLCPP_DEBUG(" "); // Read kernel file - ifstream in(ros::package::getPath("rviz_mesh_map_plugin") + CL_RAY_CAST_KERNEL_FILE); + ifstream in(ros::package::getPath("rviz_mesh_tools_plugins") + CL_RAY_CAST_KERNEL_FILE); std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - RCLCPP_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_mesh_map_plugin").c_str(), CL_RAY_CAST_KERNEL_FILE); + RCLCPP_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_mesh_tools_plugins").c_str(), CL_RAY_CAST_KERNEL_FILE); m_clProgramSources = cl::Program::Sources(1, { cast_rays_kernel.c_str(), cast_rays_kernel.length() }); @@ -734,4 +734,4 @@ void ClusterLabelTool::resetVisual() m_visual.reset(); } -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_map_plugin/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp similarity index 98% rename from rviz_mesh_map_plugin/src/ClusterLabelVisual.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index 488cc72..f17236a 100644 --- a/rviz_mesh_map_plugin/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -45,7 +45,7 @@ * Kristin Schmidt */ -#include +#include #include @@ -57,7 +57,7 @@ #include #include -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId) : m_displayContext(context), m_labelId(labelId) @@ -271,4 +271,4 @@ void ClusterLabelVisual::initMaterial() m_material->setDepthWriteEnabled(false); } -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_map_plugin/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp similarity index 96% rename from rviz_mesh_map_plugin/src/MapDisplay.cpp rename to rviz_mesh_tools_plugins/src/MapDisplay.cpp index 2c7b75e..15c1c19 100644 --- a/rviz_mesh_map_plugin/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -46,9 +46,9 @@ * Jan Philipp Vogtherr */ -#include -#include -#include +#include +#include +#include #include #include @@ -65,7 +65,7 @@ #include -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { MapDisplay::MapDisplay() { @@ -108,7 +108,7 @@ void MapDisplay::onInitialize() { std::string name = this->getName().toStdString(); - Display* display = createDisplay("rviz_mesh_map_plugin/ClusterLabel"); + Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); m_nh = std::make_shared("~"); m_nh_p = std::make_shared("~"); @@ -120,7 +120,7 @@ void MapDisplay::onInitialize() addChild(m_clusterLabelDisplay); m_clusterLabelDisplay->initialize(context_); - Display* meshDisplay = createDisplay("rviz_mesh_map_plugin/Mesh"); + Display* meshDisplay = createDisplay("rviz_mesh_tools_plugins/Mesh"); m_meshDisplay = static_cast(meshDisplay); addChild(m_meshDisplay); @@ -158,7 +158,7 @@ void MapDisplay::load(const rviz_common::Config& config) { // Override with ros params std::stringstream ss; - ss << "rviz_mesh_map_plugin/" << name; + ss << "rviz_mesh_tools_plugins/" << name; std::string mesh_file; if(m_nh_p->getParam(ss.str(), mesh_file)) @@ -219,7 +219,7 @@ bool MapDisplay::loadData() std::string name = this->getName().toStdString(); // std::stringstream ss; - // ss << "rviz_mesh_map_plugin/" << name; + // ss << "rviz_mesh_tools_plugins/" << name; // std::string mesh_file; // if(m_nh_p->getParam(ss.str(), mesh_file)) @@ -492,7 +492,7 @@ void MapDisplay::saveLabel(Cluster cluster) } } -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::MapDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MapDisplay, rviz_common::Display) diff --git a/rviz_mesh_map_plugin/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp similarity index 92% rename from rviz_mesh_map_plugin/src/MeshDisplay.cpp rename to rviz_mesh_tools_plugins/src/MeshDisplay.cpp index 97d7cd5..158bd72 100644 --- a/rviz_mesh_map_plugin/src/MeshDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp @@ -51,7 +51,7 @@ * Malte kleine Piening */ -#include +#include #include #include @@ -86,7 +86,7 @@ using namespace std::chrono_literals; using std::placeholders::_1; using std::placeholders::_2; -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { MeshDisplay::MeshDisplay() : rviz_common::Display() @@ -460,7 +460,7 @@ void MeshDisplay::updateBufferSize() void MeshDisplay::updateMesh() { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Mesh Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Mesh Display: Update"); bool showFaces = false; bool showTextures = false; @@ -545,7 +545,7 @@ void MeshDisplay::updateMesh() std::shared_ptr visual = getLatestVisual(); if (!visual) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); return; } @@ -696,7 +696,7 @@ void MeshDisplay::updateMaterialAndTextureServices() setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); } } else { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); } } @@ -755,7 +755,7 @@ void MeshDisplay::initialServiceCall() { std::string uuid = uuids[0]; - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Initial data available for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Initial data available for UUID=" << uuid); // mesh_msgs::srv::GetGeometry srv_geometry; auto req_geometry = std::make_shared(); @@ -766,15 +766,15 @@ void MeshDisplay::initialServiceCall() if(rclcpp::spin_until_future_complete(node, fut_geometry) == rclcpp::FutureReturnCode::SUCCESS) { auto res_geometry = fut_geometry.get(); - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Found geometry for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found geometry for UUID=" << uuid); processMessage(res_geometry->mesh_geometry_stamped); } else { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Could not load geometry. Waiting for callback to trigger ... "); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load geometry. Waiting for callback to trigger ... "); } } } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "No initial data available, waiting for callback to trigger ..."); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "No initial data available, waiting for callback to trigger ..."); } } @@ -791,14 +791,14 @@ void MeshDisplay::processMessage(const mesh_msgs::msg::MeshGeometryStamped& mesh if (!context_->getFrameManager()->getTransform(meshMsg.header.frame_id, meshMsg.header.stamp, position, orientation)) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Error transforming from frame '%s' to frame '%s'", meshMsg.header.frame_id.c_str(), + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Error transforming from frame '%s' to frame '%s'", meshMsg.header.frame_id.c_str(), qPrintable(rviz_common::Display::fixed_frame_)); return; } if (!m_lastUuid.empty() && meshMsg.uuid.compare(m_lastUuid) != 0) { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received geometry with new UUID!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received geometry with new UUID!"); m_costCache.clear(); m_selectVertexCostMap->clearOptions(); m_selectVertexCostMap->addOption("-- None --", 0); @@ -853,7 +853,7 @@ void MeshDisplay::incomingVertexColors( { if (colorsStamped->uuid.compare(m_lastUuid) != 0) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received vertex colors, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received vertex colors, but UUIDs dont match!"); return; } @@ -872,7 +872,7 @@ void MeshDisplay::incomingVertexCosts( { if (costsStamped->uuid.compare(m_lastUuid) != 0) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received vertex costs, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received vertex costs, but UUIDs dont match!"); return; } @@ -896,7 +896,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) if(rclcpp::spin_until_future_complete(node, fut_vertex_colors) == rclcpp::FutureReturnCode::SUCCESS) { auto res_vertex_colors = fut_vertex_colors.get(); - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Successful vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successful vertex colors service call!"); std::vector vertexColors; for (const std_msgs::msg::ColorRGBA c : res_vertex_colors->mesh_vertex_colors_stamped.mesh_vertex_colors.vertex_colors) @@ -909,7 +909,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed vertex colors service call!"); } } @@ -930,7 +930,7 @@ void MeshDisplay::requestMaterials(std::string uuid) { auto res_materials = fut_materials.get(); - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Successful materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successful materials service call!"); const mesh_msgs::msg::MeshMaterialsStamped& meshMaterialsStamped = res_materials->mesh_materials_stamped; @@ -978,7 +978,7 @@ void MeshDisplay::requestMaterials(std::string uuid) if(rclcpp::spin_until_future_complete(node, fut_texture) == rclcpp::FutureReturnCode::SUCCESS) { auto res_texture = fut_texture.get(); - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Successful texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successful texture service call with index %d!", material.texture_index); const mesh_msgs::msg::MeshTexture& textureMsg = res_texture->texture; @@ -993,31 +993,31 @@ void MeshDisplay::requestMaterials(std::string uuid) } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed texture service call with index %d!", material.texture_index); } } } } else { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Failed materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed materials service call!"); } } void MeshDisplay::cacheVertexCosts(std::string layer, const std::vector& costs) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "Cache vertex cost map '" << layer << "' for UUID "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cache vertex cost map '" << layer << "' for UUID "); // insert into cache auto it = m_costCache.find(layer); if (it != m_costCache.end()) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "The cost layer \"" << layer << "\" has been updated."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "The cost layer \"" << layer << "\" has been updated."); m_costCache.erase(layer); } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_map_plugin"), "The cost layer \"" << layer << "\" has been added."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "The cost layer \"" << layer << "\" has been added."); m_selectVertexCostMap->addOptionStd(layer, m_selectVertexCostMap->numChildren()); } @@ -1052,7 +1052,7 @@ std::shared_ptr MeshDisplay::addNewVisual() return m_visuals.back(); } -} // End namespace rviz_mesh_map_plugin +} // End namespace rviz_mesh_tools_plugins #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_map_plugin::MeshDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MeshDisplay, rviz_common::Display) diff --git a/rviz_mesh_map_plugin/src/MeshGoalTool.cpp b/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp similarity index 95% rename from rviz_mesh_map_plugin/src/MeshGoalTool.cpp rename to rviz_mesh_tools_plugins/src/MeshGoalTool.cpp index ba38342..3bbc23e 100644 --- a/rviz_mesh_map_plugin/src/MeshGoalTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp @@ -43,12 +43,12 @@ */ -#include "rviz_mesh_map_plugin/MeshGoalTool.hpp" +#include "rviz_mesh_tools_plugins/MeshGoalTool.hpp" #include -PLUGINLIB_EXPORT_CLASS( rviz_mesh_map_plugin::MeshGoalTool, rviz_common::Tool ) +PLUGINLIB_EXPORT_CLASS( rviz_mesh_tools_plugins::MeshGoalTool, rviz_common::Tool ) -namespace rviz_mesh_map_plugin{ +namespace rviz_mesh_tools_plugins{ MeshGoalTool::MeshGoalTool() { shortcut_key_ = 'm'; diff --git a/rviz_mesh_map_plugin/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp similarity index 98% rename from rviz_mesh_map_plugin/src/MeshPoseTool.cpp rename to rviz_mesh_tools_plugins/src/MeshPoseTool.cpp index 0bb4c8c..ede20f7 100644 --- a/rviz_mesh_map_plugin/src/MeshPoseTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -55,9 +55,9 @@ #include #include -#include "rviz_mesh_map_plugin/MeshPoseTool.hpp" +#include "rviz_mesh_tools_plugins/MeshPoseTool.hpp" -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { @@ -294,4 +294,4 @@ void MeshPoseTool::getRawManualObjectData(const Ogre::ManualObject* mesh, const indexBuffer->unlock(); } -} // namespace rviz_mesh_map_plugin +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_map_plugin/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp similarity index 92% rename from rviz_mesh_map_plugin/src/MeshVisual.cpp rename to rviz_mesh_tools_plugins/src/MeshVisual.cpp index 1acafb1..e5f6bda 100644 --- a/rviz_mesh_map_plugin/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -50,7 +50,7 @@ * Jan Philipp Vogtherr */ -#include "rviz_mesh_map_plugin/MeshVisual.hpp" +#include "rviz_mesh_tools_plugins/MeshVisual.hpp" #include #include @@ -64,7 +64,7 @@ #include "rclcpp/rclcpp.hpp" -namespace rviz_mesh_map_plugin +namespace rviz_mesh_tools_plugins { Ogre::ColourValue getRainbowColor1(float value) { @@ -107,7 +107,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s , m_texture_coords_enabled(false) , m_normalsScalingFactor(1) { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); // get or create the scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); @@ -161,7 +161,7 @@ MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, s MeshVisual::~MeshVisual() { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); reset(); @@ -192,7 +192,7 @@ MeshVisual::~MeshVisual() void MeshVisual::reset() { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); std::stringstream sstm; @@ -557,7 +557,7 @@ void MeshVisual::enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const float range = maxCost - minCost; if (range <= 0) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_map_plugin"), "Illegal vertex cost limits!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Illegal vertex cost limits!"); return; } @@ -654,7 +654,7 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector // if the image was only default constructed, in which case its width will be 0 if (m_images.size() < textureIndex + 1 || m_images[textureIndex].getWidth() == 0) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_map_plugin"), "Texture with index %u not loaded yet", textureIndex); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Texture with index %u not loaded yet", textureIndex); } else { @@ -783,7 +783,7 @@ bool MeshVisual::setGeometry(const Geometry& mesh) // check if there are enough vertices given if (mesh.vertices.size() < 3) { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not enough vertices, can't create mesh!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not enough vertices, can't create mesh!"); return false; } @@ -807,12 +807,12 @@ bool MeshVisual::setNormals(const vector& normals) // check if there are vertex normals for each vertex if (normals.size() == m_geometry.vertices.size()) { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex normals.", normals.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex normals.", normals.size()); m_vertex_normals_enabled = true; } else if (normals.size() > 0) { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex normals as vertices, ignoring vertex normals!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex normals as vertices, ignoring vertex normals!"); return false; } @@ -836,12 +836,12 @@ bool MeshVisual::setVertexColors(const vector& vertexColors) // check if there are vertex colors for each vertex if (vertexColors.size() == m_geometry.vertices.size()) { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex colors.", vertexColors.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex colors.", vertexColors.size()); m_vertex_colors_enabled = true; } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex colors as vertices, ignoring the vertex colors!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex colors as vertices, ignoring the vertex colors!"); return false; } @@ -867,12 +867,12 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -896,12 +896,12 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -917,12 +917,12 @@ bool MeshVisual::setMaterials(const vector& materials, const vector= 0) { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received %lu materials.", materials.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu materials.", materials.size()); m_materials_enabled = true; // enable materials } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received zero materials, ignoring materials!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received zero materials, ignoring materials!"); return false; } @@ -930,13 +930,13 @@ bool MeshVisual::setMaterials(const vector& materials, const vector 0) { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Received not as much texture coords as vertices, ignoring texture coords!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much texture coords as vertices, ignoring texture coords!"); } enteringTexturedTriangleMesh(m_geometry, materials, texCoords); @@ -965,7 +965,7 @@ bool MeshVisual::addTexture(Texture& texture, uint32_t textureIndex) } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Can't load image into texture material, material does not exist!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Can't load image into texture material, material does not exist!"); return false; } } @@ -981,7 +981,7 @@ Ogre::PixelFormat MeshVisual::getOgrePixelFormatFromRosString(std::string encodi return Ogre::PF_BYTE_RGB; } - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_map_plugin"), "Unknown texture encoding! Using Ogre::PF_UNKNOWN"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Unknown texture encoding! Using Ogre::PF_UNKNOWN"); return Ogre::PF_UNKNOWN; } @@ -1035,4 +1035,4 @@ void MeshVisual::setFrameOrientation(const Ogre::Quaternion& orientation) m_sceneNode->setOrientation(orientation); } -} // end namespace rviz_mesh_map_plugin +} // end namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_map_plugin/src/RvizFileProperty.cpp b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp similarity index 98% rename from rviz_mesh_map_plugin/src/RvizFileProperty.cpp rename to rviz_mesh_tools_plugins/src/RvizFileProperty.cpp index 654baf9..b59c7ef 100644 --- a/rviz_mesh_map_plugin/src/RvizFileProperty.cpp +++ b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp @@ -45,7 +45,7 @@ * Malte kleine Piening */ -#include "rviz_mesh_map_plugin/RvizFileProperty.hpp" +#include "rviz_mesh_tools_plugins/RvizFileProperty.hpp" #include From dd3ad2a2c5d35fae9216d52929e2f798a40dbaa3 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 10 Nov 2023 10:41:24 +0100 Subject: [PATCH 26/40] plugin xml is now equal to rviz_default_plugins --- rviz_mesh_tools_plugins/plugins_description.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rviz_mesh_tools_plugins/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml index 4e7b0d8..e87094d 100644 --- a/rviz_mesh_tools_plugins/plugins_description.xml +++ b/rviz_mesh_tools_plugins/plugins_description.xml @@ -1,4 +1,4 @@ - + From a7d94aa8a223276fe3c20c83449963c998ccd0b1 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Fri, 10 Nov 2023 11:38:44 +0100 Subject: [PATCH 27/40] ported RVizFileProperty. MeshPoseTool uses a lot of functions that have been removed. Need to figure out what functions I can use to do basic raycasting --- rviz_mesh_tools_plugins/CMakeLists.txt | 4 +- .../rviz_mesh_tools_plugins/MeshPoseTool.hpp | 10 ++- .../RvizFileProperty.hpp | 14 ++-- rviz_mesh_tools_plugins/src/MeshPoseTool.cpp | 73 ++++++++++++++++--- .../src/RvizFileProperty.cpp | 11 +-- 5 files changed, 89 insertions(+), 23 deletions(-) diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 1f8a342..4452ce9 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -54,7 +54,7 @@ set(SOURCE_FILES # src/MapDisplay.cpp src/MeshDisplay.cpp src/MeshVisual.cpp - # src/RvizFileProperty.cpp + src/RvizFileProperty.cpp # src/MeshPoseTool.cpp # src/MeshGoalTool.cpp ) @@ -69,7 +69,7 @@ set(MOC_HEADER_FILES include/MeshVisual.hpp # include/ClusterLabelTool.hpp # include/CLUtil.hpp - # include/RvizFileProperty.hpp + include/RvizFileProperty.hpp # include/MeshPoseTool.hpp # include/MeshGoalTool.hpp ) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp index a41d3f9..0551a16 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp @@ -52,7 +52,13 @@ #include #include -#include +// #include + +// Forward declare types +namespace rviz_rendering +{ + class Arrow; +} namespace rviz_mesh_tools_plugins { @@ -80,7 +86,7 @@ class MeshPoseTool : public rviz_common::Tool bool selectTriangle(rviz_common::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& orientation); - rviz_common::Arrow* arrow_; + rviz_rendering::Arrow* arrow_; enum State { Position, diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp index 8f254b0..fc6f89f 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp @@ -49,17 +49,21 @@ #define RVIZ_FILE_PROPERTY_HPP #include -#include +#include namespace rviz { -class FileProperty : public Property +class FileProperty : public rviz_common::properties::FilePickerProperty { Q_OBJECT public: - FileProperty(const QString& name = QString(), const QString& default_value = QString(), - const QString& description = QString(), Property* parent = nullptr, const char* changed_slot = nullptr, - QObject* receiver = nullptr); + FileProperty( + const QString& name = QString(), + const QString& default_value = QString(), + const QString& description = QString(), + Property* parent = nullptr, + const char* changed_slot = nullptr, + QObject* receiver = nullptr); std::string getFilename() { diff --git a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp index ede20f7..27536e1 100644 --- a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -40,6 +40,7 @@ * MeshPoseTool.cpp * * authors: Sebastian Pütz + * Alexander Mock * */ @@ -48,17 +49,51 @@ #include #include -#include -#include -#include -#include -#include +#include +#include + +// #include "rviz_rendering/render_window.hpp" + +#include +#include +#include #include +// method is private :( +#include + +#include + #include "rviz_mesh_tools_plugins/MeshPoseTool.hpp" + + namespace rviz_mesh_tools_plugins { + +bool getViewportProjectionOnPlane( + rviz_rendering::RenderWindow* render_window, + int x, int y, + const Ogre::Plane& plane, + Ogre::Vector3& intersection_point) +{ + auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_window); + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( + static_cast(x) / static_cast(width), + static_cast(y) / static_cast(height)); + + auto intersection = mouse_ray.intersects(plane); + if (!intersection.first) + { + return false; + } + + intersection_point = mouse_ray.getPoint(intersection.second); + return true; +} + MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { } @@ -70,7 +105,7 @@ MeshPoseTool::~MeshPoseTool() void MeshPoseTool::onInitialize() { - arrow_ = new rviz_common::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); arrow_->getSceneNode()->setVisible(false); } @@ -92,7 +127,9 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) if (event.leftDown()) { - RCLCPP_ASSERT(state_ == Position); + // TODO: check if there is a proper ROS2 equivalent + // RCLCPP_ASSERT(state_ == Position); + assert(state_ == Position); Ogre::Vector3 pos, ori; if (selectTriangle(event, pos, ori)) @@ -110,7 +147,14 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { Ogre::Vector3 cur_pos; Ogre::Plane plane(ori_, pos_); - if (rviz_common::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) + + auto render_window = event.panel->getRenderWindow(); + + Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( + (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); + + + if (getViewportProjectionOnPlane(render_window, event.x, event.y, plane, cur_pos)) { // right hand coordinate system // x to the right, y to the top, and -z into the scene @@ -134,9 +178,20 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { if (state_ == Orientation) { + // rviz_rendering::project3DPointToViewportXY Ogre::Vector3 cur_pos; Ogre::Plane plane(ori_, pos_); - if (rviz_common::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) + + // Start point on triangle is given. + // When releasing the left mouse button we search for + // the end point on the (infinite-sized) plane of the triangle + // + // ROS1: rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos) + // ROS2: ? + + auto render_window = event.panel->getRenderWindow(); + + if (getViewportProjectionOnPlane(render_window, event.x, event.y, plane, cur_pos)) { // arrow foreward negative z Ogre::Vector3 z_axis = -(cur_pos - pos_); diff --git a/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp index b59c7ef..5956394 100644 --- a/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp +++ b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp @@ -53,17 +53,20 @@ namespace rviz { FileProperty::FileProperty(const QString& name, const QString& default_value, const QString& description, Property* parent, const char* changed_slot, QObject* receiver) - : Property(name, default_value, description, parent, changed_slot, receiver) + : rviz_common::properties::FilePickerProperty( + name, default_value, description, parent, changed_slot, receiver) { } -QWidget* FileProperty::createEditor(QWidget* parent, const QStyleOptionViewItem&) +QWidget* FileProperty::createEditor( + QWidget* parent, + const QStyleOptionViewItem&) { QFileDialog* editor = new QFileDialog(nullptr); QStringList filenameFilters; + filenameFilters << tr("*"); filenameFilters << tr("*.h5"); - #if defined(WITH_ASSIMP) filenameFilters << tr("*.ply"); filenameFilters << tr("*.obj"); filenameFilters << tr("*.dae"); @@ -72,8 +75,6 @@ QWidget* FileProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& filenameFilters << tr("*.3ds"); filenameFilters << tr("*.fbx"); filenameFilters << tr("*.blend"); - #endif - filenameFilters << tr("*"); editor->setNameFilters(filenameFilters); editor->setViewMode(QFileDialog::Detail); From 71fd32ae0903c447a7008aaf4998c0503bf76dd7 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 13 Nov 2023 16:51:43 +0100 Subject: [PATCH 28/40] passive visualizations are working now. interactions are still missing (PoseTool, LabelTool) --- rviz_mesh_tools_plugins/CMakeLists.txt | 60 +- .../ClusterLabelDisplay.hpp | 72 ++- .../ClusterLabelTool.hpp | 68 +- .../ClusterLabelVisual.hpp | 8 +- .../rviz_mesh_tools_plugins/MapDisplay.hpp | 17 +- .../RvizFileProperty.hpp | 14 +- .../plugins_description.xml | 4 +- .../rviz_mesh_tools_plugins-extras.cmake | 3 +- .../src/ClusterLabelDisplay.cpp | 42 +- .../src/ClusterLabelPanel.cpp | 17 +- .../src/ClusterLabelTool.cpp | 612 +++++++++++------- .../src/ClusterLabelVisual.cpp | 25 +- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 154 +++-- rviz_mesh_tools_plugins/src/MeshDisplay.cpp | 5 +- rviz_mesh_tools_plugins/src/MeshVisual.cpp | 43 +- .../src/RvizFileProperty.cpp | 19 +- 16 files changed, 692 insertions(+), 471 deletions(-) diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 4452ce9..f5e6350 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -10,6 +10,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +add_definitions(-D_BUILD_DIR_PATH="${CMAKE_CURRENT_BINARY_DIR}") +add_definitions(-D_SRC_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}") + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmw REQUIRED) @@ -23,18 +26,20 @@ find_package(mesh_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(pluginlib REQUIRED) find_package(message_filters REQUIRED) +find_package(std_msgs REQUIRED) -find_package(Boost REQUIRED COMPONENTS system) +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) +find_package(OpenCL 2 REQUIRED) include_directories(${ASSIMP_INCLUDE_DIRS}) ## This setting causes Qt's "MOC" generation to happen automatically. # set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOMOC ON) find_package(Qt5 COMPONENTS Core Widgets) add_definitions(-DQT_NO_KEYWORDS) @@ -47,11 +52,11 @@ include_directories( ) set(SOURCE_FILES - # src/ClusterLabelDisplay.cpp - # src/ClusterLabelPanel.cpp - # src/ClusterLabelTool.cpp - # src/ClusterLabelVisual.cpp - # src/MapDisplay.cpp + src/ClusterLabelDisplay.cpp + src/ClusterLabelPanel.cpp + src/ClusterLabelTool.cpp + src/ClusterLabelVisual.cpp + src/MapDisplay.cpp src/MeshDisplay.cpp src/MeshVisual.cpp src/RvizFileProperty.cpp @@ -61,26 +66,31 @@ set(SOURCE_FILES set(MOC_HEADER_FILES - # include/ClusterLabelDisplay.hpp - # include/ClusterLabelPanel.hpp - # include/ClusterLabelVisual.hpp - # include/MapDisplay.hpp - include/MeshDisplay.hpp - include/MeshVisual.hpp - # include/ClusterLabelTool.hpp - # include/CLUtil.hpp - include/RvizFileProperty.hpp - # include/MeshPoseTool.hpp - # include/MeshGoalTool.hpp + include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp + include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp + include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp + include/rviz_mesh_tools_plugins/MapDisplay.hpp + 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 ) -foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") - qt5_wrap_cpp(RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES "${header}") -endforeach() +# foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") +# qt5_wrap_cpp(RVIZ_MESH_TOOLS_PLUGINS_MOC "${header}") +# endforeach() + + add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} - ${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES} + ${MOC_HEADER_FILES} + ${HDF5_LIBRARIES} + ${HDF5_HL_LIBRARIES} + ${OpenCL_LIBRARIES} ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -92,11 +102,14 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PUBLIC rviz_ogre_vendor::OgreMain rviz_ogre_vendor::OgreOverlay + Boost::system + Boost::filesystem ) 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")# @@ -115,6 +128,7 @@ ament_target_dependencies(${PROJECT_NAME} resource_retriever tf2_ros message_filters + std_msgs ) # Export old-style CMake variables @@ -149,8 +163,6 @@ install( DESTINATION include/${PROJECT_NAME} ) - - install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" DESTINATION share/${PROJECT_NAME}) 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 dd0c422..59d86ba 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 @@ -69,43 +69,47 @@ // #include // #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include -#ifndef Q_MOC_RUN -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#endif -namespace rviz +#ifndef Q_MOC_RUN +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#endif // Q_MOC_RUN + +namespace rviz_common +{ +namespace properties { // Forward declaration class BoolProperty; @@ -114,8 +118,8 @@ class FloatProperty; class IntProperty; class EnumProperty; class StringProperty; - -} // End namespace rviz +} // namespace properties +} // namespace rviz_common namespace rviz_mesh_tools_plugins { @@ -292,6 +296,6 @@ private Q_SLOTS: }; -} // end namespace rviz_mesh_tools_plugins +} // namespace rviz_mesh_tools_plugins -#endif +#endif // CLUSTER_LABEL_DISPLAY_HPP 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 ef5370e..1393201 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 @@ -48,13 +48,15 @@ #ifndef CLUSTER_LABEL_TOOL_HPP #define CLUSTER_LABEL_TOOL_HPP + + +#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 @@ -88,6 +90,7 @@ #ifndef Q_MOC_RUN #include + #include #include #include @@ -97,19 +100,26 @@ #include #include +#include + #endif // Q_MOC_RUN -namespace rviz +#include + +namespace rviz_common +{ +namespace properties { class RosTopicProperty; class ColorProperty; -} // namespace rviz +} // namespace properties +} // namespace rviz_common // OGRE stuff namespace Ogre { // Forward declaration -class Vector3; +// class Vector3; } // namespace Ogre namespace rviz_mesh_tools_plugins @@ -214,8 +224,18 @@ public Q_SLOTS: Ogre::SceneNode* m_sceneNode; Ogre::ManualObject* m_selectionBox; Ogre::MaterialPtr m_selectionBoxMaterial; - Ogre::Vector2 m_selectionStart; - Ogre::Vector2 m_selectionStop; + + // rviz_common::ViewportMouseEvent m_evt_start; + // rviz_common::ViewportMouseEvent m_evt_stop; + + int m_bb_x1; + int m_bb_y1; + int m_bb_x2; + int m_bb_y2; + + rviz_common::RenderPanel* m_evt_panel; + + bool m_multipleSelect = false; bool m_singleSelect = false; bool m_singleDeselect = false; @@ -233,7 +253,7 @@ public Q_SLOTS: void selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode); boost::optional> getClosestIntersectedFaceParallel(Ogre::Ray& ray); - ros::Publisher m_labelPublisher; + rclcpp::Publisher::SharedPtr m_labelPublisher; std::vector m_vertexData; std::array m_rayData; @@ -243,21 +263,21 @@ public Q_SLOTS: 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; + // 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; }; } // 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 a1ce6ec..810ad07 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 @@ -56,10 +56,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp index 28dc45d..7fadb30 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp @@ -71,8 +71,8 @@ #include #include -#include -#include +// #include +// #include #include #include @@ -106,13 +106,15 @@ #include #include -#endif +#endif // Q_MOCK_RUN #include #include namespace rviz { +namespace properties +{ // Forward declaration class BoolProperty; class ColorProperty; @@ -120,8 +122,8 @@ class FloatProperty; class IntProperty; class EnumProperty; class StringProperty; - -} // End namespace rviz +} // namespace properties +} // namespace rviz namespace rviz_mesh_tools_plugins { @@ -212,11 +214,8 @@ private Q_SLOTS: std::map> m_costs; - std::shared_ptr m_nh; - std::shared_ptr m_nh_p; - /// Path to map file - rviz_common::FileProperty* m_mapFilePath; + rviz_common::properties::FileProperty* m_mapFilePath; std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp index fc6f89f..89ae3e0 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp @@ -43,6 +43,7 @@ * authors: * * Malte kleine Piening + * Alexander Mock */ #ifndef RVIZ_FILE_PROPERTY_HPP @@ -51,9 +52,13 @@ #include #include -namespace rviz +namespace rviz_common { -class FileProperty : public rviz_common::properties::FilePickerProperty + +namespace properties +{ + +class FileProperty : public FilePickerProperty { Q_OBJECT public: @@ -83,6 +88,7 @@ public Q_SLOTS: } }; -} // end namespace rviz +} // namespace properties +} // namespace rviz -#endif +#endif // RVIZ_FILE_PROPERTY_HPP diff --git a/rviz_mesh_tools_plugins/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml index e87094d..e401036 100644 --- a/rviz_mesh_tools_plugins/plugins_description.xml +++ b/rviz_mesh_tools_plugins/plugins_description.xml @@ -34,11 +34,11 @@ Displays a mesh. - Select a goal on a mesh. - + --> diff --git a/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake b/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake index 2724ff1..d07309a 100644 --- a/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake +++ b/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake @@ -43,4 +43,5 @@ # # Alexander Mock # -find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) \ No newline at end of file +find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) +find_package(Boost REQUIRED QUITE COMPONENTS system filesystem) \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 2bb690e..bda55dd 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -50,12 +50,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" @@ -103,16 +103,16 @@ ClusterLabelDisplay::ClusterLabelDisplay() m_alphaProperty->setMin(0.0f); m_alphaProperty->setMax(1.0f); - m_colorsProperty = new rviz_common::Property("Colors", "", "colors", this, SLOT(updateColors()), this); + m_colorsProperty = new rviz_common::properties::Property("Colors", "", "colors", this, SLOT(updateColors()), this); m_colorsProperty->setReadOnly(true); m_sphereSizeProperty = - new rviz_common::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); + new rviz_common::properties::FloatProperty("Brush Size", 1.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", this, SLOT(updatePhantomVisual()), this); - setStatus(rviz_common::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); + setStatus(rviz_common::properties::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); } ClusterLabelDisplay::~ClusterLabelDisplay() @@ -151,7 +151,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector if (isEnabled()) updateMap(); - setStatus(rviz_common::StatusProperty::Ok, "Display", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); } // ===================================================================================================================== @@ -159,8 +159,10 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector void ClusterLabelDisplay::onInitialize() { + std::cout << "ClusterLabelDisplay::onInitialize()" << std::endl; // Look for an existing label tool or create a new one initializeLabelTool(); + std::cout << "-DONE" << std::endl; } void ClusterLabelDisplay::onEnable() @@ -226,7 +228,7 @@ void ClusterLabelDisplay::updateMap() m_tool->setDisplay(this); // All good - setStatus(rviz_common::StatusProperty::Ok, "Map", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); } void ClusterLabelDisplay::updateColors() @@ -269,7 +271,7 @@ void ClusterLabelDisplay::fillPropertyOptions() // Add color options Ogre::ColourValue rainbowColor = getRainbowColor((((float)i + 1) / m_clusterList.size())); - m_colorProperties.emplace_back(new rviz_common::ColorProperty( + m_colorProperties.emplace_back(new rviz_common::properties::ColorProperty( QString::fromStdString(m_clusterList[i].name), QColor(rainbowColor.r * 255, rainbowColor.g * 255, rainbowColor.b * 255), QString::fromStdString(m_clusterList[i].name), m_colorsProperty, SLOT(updateColors()), this)); @@ -325,8 +327,9 @@ void ClusterLabelDisplay::initializeLabelTool() bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) { - if (toolClasses.at(i).contains("ClusterLabel")) + if (toolClasses[i].contains("ClusterLabel")) { + std::cout << "Convert " << toolClasses[i].toStdString() << std::endl; m_tool = static_cast(toolManager->getTool(i)); foundTool = true; break; @@ -335,7 +338,16 @@ void ClusterLabelDisplay::initializeLabelTool() if (!foundTool) { - m_tool = static_cast(context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); + std::cout << "Create new tool" << std::endl; + auto bla = context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel"); + std::cout << "bla" << std::endl; + + if(m_tool = dynamic_cast(bla); m_tool != nullptr) + { + std::cout << "SUCCESS" << std::endl; + } else { + std::cout << "Could not create ClusterLabelTool :(" << std::endl; + } } } @@ -353,5 +365,5 @@ void ClusterLabelDisplay::addLabel(std::string label, std::vector face } // End namespace rviz_mesh_tools_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelDisplay, rviz_common::Display) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp index 0958011..79c8cec 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp @@ -56,11 +56,12 @@ #include #include -#include +#include namespace rviz_mesh_tools_plugins { -ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz_common::Panel(parent) +ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) +:rviz_common::Panel(parent) { QHBoxLayout* clusterNameLayout = new QHBoxLayout(); clusterNameLayout->addWidget(new QLabel("Cluster Name:")); @@ -87,7 +88,8 @@ ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz_common::Panel(paren void ClusterLabelPanel::onInitialize() { // Check if the cluster label tool is already opened - rviz_common::ToolManager* toolManager = vis_manager_->getToolManager(); + + rviz_common::ToolManager* toolManager = this->getDisplayContext()->getToolManager(); QStringList toolClasses = toolManager->getToolClasses(); bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) @@ -102,7 +104,7 @@ void ClusterLabelPanel::onInitialize() if (!foundTool) { - m_tool = static_cast(vis_manager_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); + m_tool = static_cast(this->getDisplayContext()->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); } } @@ -122,13 +124,13 @@ void ClusterLabelPanel::updateClusterName() void ClusterLabelPanel::publish() { - RCLCPP_INFO("Label Panel: Publish"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Panel: Publish"); m_tool->publishLabel(m_clusterName.toStdString()); } void ClusterLabelPanel::resetFaces() { - RCLCPP_INFO("Label panel: Reset"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label panel: Reset"); m_tool->resetFaces(); } @@ -143,7 +145,6 @@ void ClusterLabelPanel::load(const rviz_common::Config& config) rviz_common::Panel::load(config); QString clusterName; if (config.mapGetString("ClusterName", &clusterName)) - ; { m_clusterNameEditor->setText(clusterName); updateClusterName(); @@ -152,5 +153,5 @@ void ClusterLabelPanel::load(const rviz_common::Config& config) } // End namespace rviz_mesh_tools_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelPanel, rviz_common::Panel) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 2d213ff..61dcdcd 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -57,12 +57,20 @@ #include -#include +#include +#include + +#include + +#include #include +#include +#include + +#include + -#include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelTool, rviz_common::Tool) using std::ifstream; using std::stringstream; @@ -71,12 +79,16 @@ namespace rviz_mesh_tools_plugins { #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" -ClusterLabelTool::ClusterLabelTool() : m_displayInitialized(false) +ClusterLabelTool::ClusterLabelTool() +:rviz_common::Tool() +,m_displayInitialized(false) +// ,m_evt_start(nullptr, (QMouseEvent*)nullptr, 0, 0) +// ,m_evt_stop(nullptr, (QMouseEvent*)nullptr, 0, 0) { shortcut_key_ = 'l'; - ros::NodeHandle n; - m_labelPublisher = n.advertise("/cluster_label", 1, true); + // ros::NodeHandle n; + // m_labelPublisher = n.advertise("/cluster_label", 1, true); } ClusterLabelTool::~ClusterLabelTool() @@ -91,7 +103,12 @@ ClusterLabelTool::~ClusterLabelTool() // context_ are set. It should be called only once per instantiation. void ClusterLabelTool::onInitialize() { - RCLCPP_DEBUG("ClusterLabelTool: Call Init"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "ClusterLabelTool: Call Init"); + + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + m_labelPublisher = node->create_publisher( + "/cluster_label", 1); + m_sceneNode = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode(); m_selectionBox = context_->getSceneManager()->createManualObject("ClusterLabelTool_SelectionBox"); @@ -111,121 +128,132 @@ void ClusterLabelTool::onInitialize() m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); - // try-catch block to check for OpenCL errors - try - { - // Initialize OpenCL - RCLCPP_DEBUG("Get platforms"); - vector platforms; - cl::Platform::get(&platforms); - for (auto const& platform : platforms) - { - RCLCPP_DEBUG("Found platform: %s", platform.getInfo().c_str()); - RCLCPP_DEBUG("platform version: %s", platform.getInfo().c_str()); - } - RCLCPP_DEBUG(" "); - - vector consideredDevices; - for (auto const& platform : platforms) - { - RCLCPP_DEBUG("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("Found device: %s", device.getInfo().c_str()); - RCLCPP_DEBUG("Device work units: %d", device.getInfo()); - RCLCPP_DEBUG("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("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(" "); + // std::cout << "ClusterLabelTool INIT kdwioh" << std::endl; - 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("No device with compatible OpenCL version found (minimum 2.0)"); - ros::requestShutdown(); - } - - cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; - m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); - - RCLCPP_INFO("Using device %s of platform %s", m_clDevice.getInfo().c_str(), - platform.getInfo().c_str()); - RCLCPP_DEBUG(" "); - - // Read kernel file - ifstream in(ros::package::getPath("rviz_mesh_tools_plugins") + CL_RAY_CAST_KERNEL_FILE); - std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - - RCLCPP_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_mesh_tools_plugins").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("Successfully built program."); - } - catch (cl::Error& err) - { - RCLCPP_ERROR("Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); - - 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(err.what() << ": " << CLUtil::getErrorString(err.err())); - RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - ros::requestShutdown(); - exit(1); - } + // try-catch block to check for OpenCL errors + // try + // { + // // Initialize OpenCL + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Get platforms"); + // vector platforms; + // cl::Platform::get(&platforms); + // for (auto const& platform : platforms) + // { + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Found platform: %s", platform.getInfo().c_str()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "platform version: %s", platform.getInfo().c_str()); + // } + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), " "); + + // vector consideredDevices; + // for (auto const& platform : platforms) + // { + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "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_plugin"), "Found device: %s", device.getInfo().c_str()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Device work units: %d", device.getInfo()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "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_plugin"), "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_plugin"), " "); + + // 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_plugin"), "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_plugin"), "Using device %s of platform %s", m_clDevice.getInfo().c_str(), + // platform.getInfo().c_str()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), " "); + + // // 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_plugin"), "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_plugin"), "Successfully built program."); + // } + // catch (cl::Error& err) + // { + + // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "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_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + // rclcpp::shutdown(); + // // ros::requestShutdown(); + // exit(1); + // } } void ClusterLabelTool::setVisual(std::shared_ptr visual) @@ -246,7 +274,7 @@ void ClusterLabelTool::setVisual(std::shared_ptr visual) void ClusterLabelTool::setSphereSize(float size) { - m_clKernelSphere.setArg(3, size); + // m_clKernelSphere.setArg(3, size); m_sphereSize = size; } @@ -283,56 +311,59 @@ 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); - - 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(err.what() << ": " << CLUtil::getErrorString(err.err())); - RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - ros::shutdown(); - exit(1); - } + // 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_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + // rclcpp::shutdown(); + // // ros::shutdown(); + // exit(1); + // } } void ClusterLabelTool::updateSelectionBox() { + // TODO CHECK THIS float left, right, top, bottom; - left = m_selectionStart.x * 2 - 1; - right = m_selectionStop.x * 2 - 1; - top = 1 - m_selectionStart.y * 2; - bottom = 1 - m_selectionStop.y * 2; + // left = m_evt_start.x * 2 - 1; + // right = m_evt_stop.x * 2 - 1; + // top = 1 - m_evt_start.y * 2; + // bottom = 1 - m_evt_stop.y * 2; m_selectionBox->clear(); m_selectionBox->begin(m_selectionBoxMaterial->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); @@ -347,28 +378,52 @@ void ClusterLabelTool::updateSelectionBox() void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) { - m_selectionStart.x = (float)event.x / event.viewport->getActualWidth(); - m_selectionStart.y = (float)event.y / event.viewport->getActualHeight(); - m_selectionStop = m_selectionStart; + // 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; + + + m_bb_x2 = m_bb_x1; + m_bb_y2 = m_bb_y1; m_selectionBox->clear(); m_selectionBox->setVisible(true); } 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(); + // 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; + updateSelectionBox(); } -void ClusterLabelTool::selectMultipleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode) +void ClusterLabelTool::selectMultipleFaces( + rviz_common::ViewportMouseEvent& event, bool selectMode) { m_selectionBox->setVisible(false); - float left = m_selectionStart.x; - float right = m_selectionStop.x; - float top = m_selectionStart.y; - float bottom = m_selectionStop.y; + 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; @@ -383,17 +438,57 @@ void ClusterLabelTool::selectMultipleFaces(rviz_common::ViewportMouseEvent& even std::swap(top, bottom); } - const float BOX_SIZE_TOLERANCE = 0.0001; + const int BOX_SIZE_TOLERANCE = 1; if ((right - left) * (bottom - top) < BOX_SIZE_TOLERANCE) { selectSingleFace(event, selectMode); return; } - Ogre::PlaneBoundedVolume volume = - event.viewport->getCamera()->getCameraToViewportBoxVolume(left, top, right, bottom, true); + // 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; + + rviz_common::interaction::M_Picked pick_results; + + auto manager = context_->getSelectionManager(); + + manager->pick( + m_evt_panel->getRenderWindow(), + m_bb_x1, m_bb_y1, + m_bb_x2, m_bb_y2, + pick_results); + + if(!pick_results.empty()) + { + // FOUND SOMETHING! + std::cout << "FOUND SOMETHING!" << std::endl; + + for(auto elem : pick_results) + { + rviz_common::interaction::CollObjectHandle key = elem.first; + rviz_common::interaction::Picked value = elem.second; + + std::cout << "Extra handles: " << value.extra_handles.size() << std::endl; + + + } + + + // TODO: continue implementing this + } - selectFacesInBoxParallel(volume, selectMode); } void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode) @@ -407,23 +502,23 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume m_boxData.push_back(plane.d); } - try - { - m_clQueue.enqueueWriteBuffer(m_clBoxBuffer, CL_TRUE, 0, sizeof(float) * 4 * 6, m_boxData.data()); + // 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_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(err.what() << ": " << CLUtil::getErrorString(err.err())); - RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } + // 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_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) { @@ -456,8 +551,13 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume void ClusterLabelTool::selectSingleFace(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.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() + // ); + Ogre::Ray ray; + throw std::runtime_error("TODO"); selectSingleFaceParallel(ray, selectMode); } @@ -468,23 +568,23 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) std::vector> intersectedFaceList; - try - { - m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); + // 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_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(err.what() << ": " << CLUtil::getErrorString(err.err())); - RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } + // 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_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } int closestFaceId = -1; float minDist = std::numeric_limits::max(); @@ -518,18 +618,26 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) m_visual->setFacesInCluster(tmpFaceList); - RCLCPP_DEBUG("selectSingleFaceParallel() found face with id %d", closestFaceId); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "selectSingleFaceParallel() found face with id %d", closestFaceId); } } -void ClusterLabelTool::selectSphereFaces(rviz_common::ViewportMouseEvent& event, bool selectMode) +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.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) +void ClusterLabelTool::selectSphereFacesParallel( + Ogre::Ray& ray, bool selectMode) { auto raycastResult = getClosestIntersectedFaceParallel(ray); @@ -539,23 +647,23 @@ void ClusterLabelTool::selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode 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()); + // 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_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(err.what() << ": " << CLUtil::getErrorString(err.err())); - RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } + // 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_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) { @@ -586,28 +694,29 @@ void ClusterLabelTool::selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode } } -boost::optional> ClusterLabelTool::getClosestIntersectedFaceParallel(Ogre::Ray& ray) +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()); + // 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_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(err.what() << ": " << CLUtil::getErrorString(err.err())); - RCLCPP_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } + // 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_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } uint32_t closestFaceId; bool faceFound = false; @@ -635,7 +744,7 @@ boost::optional> ClusterLabelTool::getClosestIntersec void ClusterLabelTool::publishLabel(std::string label) { - RCLCPP_DEBUG_STREAM("Label Tool: Publish label '" << label << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Label Tool: Publish label '" << label << "'"); vector faces; for (uint32_t i = 0; i < m_faceSelectedArray.size(); i++) @@ -735,3 +844,8 @@ void ClusterLabelTool::resetVisual() } } // End namespace rviz_mesh_tools_plugins + + + +#include +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelTool, rviz_common::Tool) \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index f17236a..aef7363 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -49,13 +49,14 @@ #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include namespace rviz_mesh_tools_plugins { @@ -163,7 +164,7 @@ ClusterLabelVisual::~ClusterLabelVisual() if (!m_mesh.isNull()) { - RCLCPP_DEBUG("ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); try { @@ -171,13 +172,13 @@ ClusterLabelVisual::~ClusterLabelVisual() } catch (...) { - RCLCPP_ERROR("Exception in Visual destructor"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Exception in Visual destructor"); } } if (m_sceneNode->numAttachedObjects() == 0) { - RCLCPP_INFO("ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); } } @@ -201,7 +202,7 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) if (!m_geometry) { - RCLCPP_WARN("ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); return; } @@ -212,7 +213,7 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) m_subMesh->indexData->indexCount = 0; m_subMesh->indexData->indexStart = 0; m_material->getTechnique(0)->removeAllPasses(); - RCLCPP_DEBUG("ClusterLabelVisual::setFacesInCluster: faces empty!"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: faces empty!"); return; } diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 15c1c19..a668a8b 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -50,15 +50,17 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include +#include +#include +#include + +// #include +// #include +#include +#include #include #include @@ -69,7 +71,7 @@ namespace rviz_mesh_tools_plugins { MapDisplay::MapDisplay() { - m_mapFilePath = new rviz_common::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, + m_mapFilePath = new rviz_common::properties::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, SLOT(updateMap())); } @@ -84,7 +86,7 @@ std::shared_ptr MapDisplay::getGeometry() { if (!m_geometry) { - RCLCPP_ERROR("Map Display: Geometry requested, but none available!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Geometry requested, but none available!"); } return m_geometry; } @@ -94,12 +96,20 @@ std::shared_ptr MapDisplay::getGeometry() rviz_common::Display* MapDisplay::createDisplay(const QString& class_id) { - rviz_common::DisplayFactory* factory = context_->getDisplayFactory(); - QString error; - rviz_common::Display* disp = factory->make(class_id, &error); + // rviz_common::DisplayFactory* factory = context_->getDisplayFactory(); + // QString error; + + // rviz_common::Display* disp = factory->make(class_id, &error); + + // rviz_common::Display* disp = context_->getDisplayFactory()->make(class_id, &error); + + rviz_common::Display* disp = context_->getRootDisplayGroup()->createDisplay(class_id); + if (!disp) { - return new rviz_common::FailedDisplay(class_id, error); + + std::cerr << "ERROR: IM SEARCHING FOR rviz_common::FailedDisplay" << std::endl; + // return new rviz_common::FailedDisplay(class_id, error); } return disp; } @@ -108,30 +118,39 @@ void MapDisplay::onInitialize() { std::string name = this->getName().toStdString(); - Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); - - m_nh = std::make_shared("~"); - m_nh_p = std::make_shared("~"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); - m_clusterLabelDisplay = static_cast(display); - m_clusterLabelDisplay->setName("ClusterLabel"); - m_clusterLabelDisplay->setModel(model_); - m_clusterLabelDisplay->setParent(this); - addChild(m_clusterLabelDisplay); - m_clusterLabelDisplay->initialize(context_); + Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); + if (m_clusterLabelDisplay = dynamic_cast(display); m_clusterLabelDisplay != nullptr) + { + m_clusterLabelDisplay->setName("ClusterLabel"); + m_clusterLabelDisplay->setModel(model_); + m_clusterLabelDisplay->setParent(this); + addChild(m_clusterLabelDisplay); + m_clusterLabelDisplay->initialize(context_); + + // Make signal/slot connections + connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. CREATED"); + } else { + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. NOT FOUND"); + } + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh"); Display* meshDisplay = createDisplay("rviz_mesh_tools_plugins/Mesh"); - - m_meshDisplay = static_cast(meshDisplay); - addChild(m_meshDisplay); - m_meshDisplay->setName("Mesh"); - m_meshDisplay->setModel(model_); - m_meshDisplay->setParent(this); - m_meshDisplay->initialize(context_); - m_meshDisplay->ignoreIncomingMessages(); - - // Make signal/slot connections - connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); + if(m_meshDisplay = dynamic_cast(meshDisplay); m_meshDisplay != nullptr) + { + addChild(m_meshDisplay); + m_meshDisplay->setName("Mesh"); + m_meshDisplay->setModel(model_); + m_meshDisplay->setParent(this); + m_meshDisplay->initialize(context_); + m_meshDisplay->ignoreIncomingMessages(); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. CREATED"); + } else { + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. NOT FOUND"); + } + } void MapDisplay::onEnable() @@ -161,7 +180,10 @@ void MapDisplay::load(const rviz_common::Config& config) ss << "rviz_mesh_tools_plugins/" << name; std::string mesh_file; - if(m_nh_p->getParam(ss.str(), mesh_file)) + + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + if(node->get_parameter(ss.str(), mesh_file)) { config2.mapSetValue(m_mapFilePath->getName(), QString::fromStdString(mesh_file) ); } else { @@ -205,7 +227,7 @@ void MapDisplay::updateMap() m_clusterLabelDisplay->setData(m_geometry, m_clusterList); // All good - setStatus(rviz_common::StatusProperty::Ok, "Map", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); m_map_file_loaded = m_mapFilePath->getFilename(); } @@ -243,33 +265,33 @@ bool MapDisplay::loadData() std::string mapFile = m_mapFilePath->getFilename(); if (mapFile.empty()) { - RCLCPP_WARN_STREAM("Map Display: No map file path specified!"); - setStatus(rviz_common::StatusProperty::Warn, "Map", "No map file path specified!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: No map file path specified!"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "No map file path specified!"); return false; } if (!boost::filesystem::exists(mapFile)) { - RCLCPP_WARN_STREAM("Map Display: Specified map file does not exist!"); - setStatus(rviz_common::StatusProperty::Warn, "Map", "Specified map file does not exist!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Specified map file does not exist!"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "Specified map file does not exist!"); return false; } - RCLCPP_INFO_STREAM("Map Display: Loading data for map '" << mapFile << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Loading data for map '" << mapFile << "'"); try { if (boost::filesystem::extension(mapFile).compare(".h5") == 0) { - RCLCPP_INFO("Map Display: Load HDF5 map"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load HDF5 map"); // Open file IO hdf5_map_io::HDF5MapIO map_io(mapFile); - RCLCPP_INFO("Map Display: Load geometry"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load geometry"); // Read geometry m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); - RCLCPP_INFO("Map Display: Load textures"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load textures"); // Read textures vector textures = map_io.getTextures(); @@ -287,7 +309,7 @@ bool MapDisplay::loadData() m_textures[textureIndex].pixelFormat = "rgb8"; } - RCLCPP_INFO("Map Display: Load materials"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load materials"); // Read materials vector materials = map_io.getMaterials(); @@ -321,7 +343,7 @@ bool MapDisplay::loadData() m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); } - RCLCPP_INFO("Map Display: Load vertex colors"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex colors"); // Read vertex colors vector colors = map_io.getVertexColors(); @@ -333,7 +355,7 @@ bool MapDisplay::loadData() m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); } - RCLCPP_INFO("Map Display: Load vertex normals"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex normals"); // Read vertex normals vector normals = map_io.getVertexNormals(); @@ -344,7 +366,7 @@ bool MapDisplay::loadData() m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); } - RCLCPP_INFO("Map Display: Load texture coordinates"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load texture coordinates"); // Read tex cords vector texCoords = map_io.getVertexTextureCoords(); @@ -355,7 +377,7 @@ bool MapDisplay::loadData() m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); } - RCLCPP_INFO("Map Display: Load clusters"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load clusters"); // Read labels m_clusterList.clear(); @@ -383,11 +405,10 @@ bool MapDisplay::loadData() } catch (const hf::DataSpaceException& e) { - RCLCPP_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load channel " << costlayer << " as a costlayer!"); } } } - #if defined(WITH_ASSIMP) else { std::cout << "LOADING WITH ASSIMP" << std::endl; @@ -419,6 +440,8 @@ bool MapDisplay::loadData() m_geometry->vertices.resize(amesh->mNumVertices); m_geometry->faces.resize(amesh->mNumFaces); + std::cout << "- Vertices, Faces: " << amesh->mNumVertices << ", " << amesh->mNumFaces << std::endl; + for (int i = 0; i < amesh->mNumVertices; i++) { m_geometry->vertices[i].x = amesh->mVertices[i].x; @@ -432,19 +455,20 @@ bool MapDisplay::loadData() m_geometry->faces[i].vertexIndices[1] = amesh->mFaces[i].mIndices[1]; m_geometry->faces[i].vertexIndices[2] = amesh->mFaces[i].mIndices[2]; } + + m_costs.clear(); } - #endif // defined(WITH_ASSIMP) } catch (...) { - RCLCPP_ERROR_STREAM("An unexpected error occurred while using Pluto Map IO"); - setStatus(rviz_common::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "An unexpected error occurred while using Pluto Map IO"); + setStatus(rviz_common::properties::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); return false; } - setStatus(rviz_common::StatusProperty::Ok, "IO", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "IO", ""); - RCLCPP_INFO("Map Display: Successfully loaded map."); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Successfully loaded map."); return true; } @@ -457,7 +481,7 @@ void MapDisplay::saveLabel(Cluster cluster) std::string label = cluster.name; std::vector faces = cluster.faces; - RCLCPP_INFO_STREAM("Map Display: add label '" << label << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: add label '" << label << "'"); try { @@ -466,8 +490,8 @@ void MapDisplay::saveLabel(Cluster cluster) boost::split(results, label, [](char c) { return c == '_'; }); if (results.size() != 2) { - RCLCPP_ERROR_STREAM("Map Display: Illegal label name '" << label << "'"); - setStatus(rviz_common::StatusProperty::Error, "Label", "Illegal label name!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Illegal label name '" << label << "'"); + setStatus(rviz_common::properties::StatusProperty::Error, "Label", "Illegal label name!"); return; } @@ -480,19 +504,19 @@ void MapDisplay::saveLabel(Cluster cluster) // Add to cluster list m_clusterList.push_back(Cluster(label, faces)); - setStatus(rviz_common::StatusProperty::Ok, "Label", "Successfully saved label"); - RCLCPP_INFO_STREAM("Map Display: Successfully added label to map."); + 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."); // update the map to show the new label updateMap(); } catch (...) { - setStatus(rviz_common::StatusProperty::Error, "Label", "Error while saving label"); + setStatus(rviz_common::properties::StatusProperty::Error, "Label", "Error while saving label"); } } } // End namespace rviz_mesh_tools_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MapDisplay, rviz_common::Display) diff --git a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp index 158bd72..0a9ceb4 100644 --- a/rviz_mesh_tools_plugins/src/MeshDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp @@ -778,7 +778,8 @@ void MeshDisplay::initialServiceCall() } } -void MeshDisplay::processMessage(const mesh_msgs::msg::MeshGeometryStamped& meshMsg) +void MeshDisplay::processMessage( + const mesh_msgs::msg::MeshGeometryStamped& meshMsg) { if (m_ignoreMsgs) { @@ -1055,4 +1056,4 @@ std::shared_ptr MeshDisplay::addNewVisual() } // End namespace rviz_mesh_tools_plugins #include -PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MeshDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MeshDisplay, rviz_common::Display) \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp index e5f6bda..e0c08e9 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -197,42 +197,61 @@ void MeshVisual::reset() std::stringstream sstm; sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "GeneralMaterial_"; - Ogre::MaterialManager::getSingleton().unload(sstm.str()); - Ogre::MaterialManager::getSingleton().remove(sstm.str()); + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str())) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + std::cout << "Could not find material '" << sstm.str() << "' to onload. skipping" << std::endl; + } + sstm.str(""); sstm.clear(); if (m_vertex_colors_enabled) { sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "Material_" << 1; - Ogre::MaterialManager::getSingleton().unload(sstm.str()); - Ogre::MaterialManager::getSingleton().remove(sstm.str()); + + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str())) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + std::cout << "Could not find material '" << sstm.str() << "' to unload. skipping" << std::endl; + } + sstm.str(""); sstm.clear(); } sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "NormalMaterial"; - Ogre::MaterialManager::getSingleton().unload(sstm.str()); - Ogre::MaterialManager::getSingleton().remove(sstm.str()); + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str())) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + std::cout << "Could not find material '" << sstm.str() << "' to unload. skipping" << std::endl; + } + sstm.str(""); sstm.clear(); for (Ogre::MaterialPtr textureMaterial : m_textureMaterials) { - Ogre::MaterialManager::getSingleton().unload(textureMaterial->getName()); - Ogre::MaterialManager::getSingleton().remove(textureMaterial->getName()); + textureMaterial->unload(); + Ogre::MaterialManager::getSingleton().remove(textureMaterial); } if (!m_noTexCluMaterial.isNull()) { - Ogre::MaterialManager::getSingleton().unload(m_noTexCluMaterial->getName()); - Ogre::MaterialManager::getSingleton().remove(m_noTexCluMaterial->getName()); + m_noTexCluMaterial->unload(); + Ogre::MaterialManager::getSingleton().remove(m_noTexCluMaterial); } if (!m_vertexCostMaterial.isNull()) { - Ogre::MaterialManager::getSingleton().unload(m_vertexCostMaterial->getName()); - Ogre::MaterialManager::getSingleton().remove(m_vertexCostMaterial->getName()); + m_vertexCostMaterial->unload(); + Ogre::MaterialManager::getSingleton().remove(m_vertexCostMaterial); } m_mesh->clear(); diff --git a/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp index 5956394..2651f9f 100644 --- a/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp +++ b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp @@ -49,12 +49,18 @@ #include -namespace rviz +namespace rviz_common { -FileProperty::FileProperty(const QString& name, const QString& default_value, const QString& description, - Property* parent, const char* changed_slot, QObject* receiver) - : rviz_common::properties::FilePickerProperty( - name, default_value, description, parent, changed_slot, receiver) + +namespace properties +{ + +FileProperty::FileProperty( + const QString& name, const QString& default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + : FilePickerProperty( + name, default_value, description, + parent, changed_slot, receiver) { } @@ -95,4 +101,5 @@ QWidget* FileProperty::createEditor( return nullptr; } -} // namespace rviz +} // namespace properties +} // namespace rviz_common From 5eae862d0cac0208fc6bc947ef2857d84b8160d6 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 13 Nov 2023 17:00:30 +0100 Subject: [PATCH 29/40] added todos --- .../ClusterLabelTool.hpp | 1 - rviz_mesh_tools_plugins/src/MapDisplay.cpp | 20 +++---------------- 2 files changed, 3 insertions(+), 18 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 1393201..ba4b8e6 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 @@ -235,7 +235,6 @@ public Q_SLOTS: rviz_common::RenderPanel* m_evt_panel; - bool m_multipleSelect = false; bool m_singleSelect = false; bool m_singleDeselect = false; diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index a668a8b..76f05b4 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -237,21 +237,7 @@ void MapDisplay::updateMap() bool MapDisplay::loadData() { - - std::string name = this->getName().toStdString(); - // std::stringstream ss; - // ss << "rviz_mesh_tools_plugins/" << name; - - // std::string mesh_file; - // if(m_nh_p->getParam(ss.str(), mesh_file)) - // { - // std::cout<< name << ": FOUND INITIAL MESH IN PARAMS - " << mesh_file << std::endl; - // m_mapFilePath->setFilename(QString::fromStdString(mesh_file)); - // } else { - // std::cout << name << ": COULDN'T FOUND MESH TO LOAD" << std::endl; - // } - if(m_mapFilePath->getFilename() == m_map_file_loaded) { @@ -259,8 +245,6 @@ bool MapDisplay::loadData() return true; } - - // Read map file path std::string mapFile = m_mapFilePath->getFilename(); if (mapFile.empty()) @@ -420,7 +404,9 @@ bool MapDisplay::loadData() // 1. scene graphs will not be imported properly. // Someone has to do some transformations according to the // node graph in the assimp structures. Or optionally (even better): - // create tf-transformations for every element of the scene graph + // create tf-transformations for every element of the scene graph# + // 2. HDF5 is used to store more information such as label etc. + // So we possibly need to transform the geometry from PLY, OBJ, DAE to H5 first?? // Assimp::Importer io; io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); From d275a9333b36a93f028a6b66736d63f50b66cb9d Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 13 Nov 2023 17:28:26 +0100 Subject: [PATCH 30/40] disabled cluster tool for none-h5 files --- .../rviz_mesh_tools_plugins/MapDisplay.hpp | 6 + rviz_mesh_tools_plugins/src/MapDisplay.cpp | 164 +++++++++++++----- 2 files changed, 125 insertions(+), 45 deletions(-) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp index 7fadb30..24b741b 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp @@ -175,6 +175,10 @@ private Q_SLOTS: void updateMap(); private: + void enableClusterLabelDisplay(); + void disableClusterLabelDisplay(); + void enableMeshDisplay(); + void disableMeshDisplay(); /** * @brief RViz callback on initialize */ @@ -196,6 +200,8 @@ private Q_SLOTS: */ bool loadData(); + + // TODO: make more efficient - currently everything is stored in the MapDisplay, the MeshDisplay and the MeshVisual /// Geometry shared_ptr m_geometry; diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 76f05b4..52a8028 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -70,6 +70,8 @@ namespace rviz_mesh_tools_plugins { MapDisplay::MapDisplay() +:m_clusterLabelDisplay(nullptr) +,m_meshDisplay(nullptr) { m_mapFilePath = new rviz_common::properties::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, SLOT(updateMap())); @@ -114,55 +116,110 @@ rviz_common::Display* MapDisplay::createDisplay(const QString& class_id) return disp; } -void MapDisplay::onInitialize() +void MapDisplay::enableClusterLabelDisplay() { - std::string name = this->getName().toStdString(); - - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); - - Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); - if (m_clusterLabelDisplay = dynamic_cast(display); m_clusterLabelDisplay != nullptr) + if(!m_clusterLabelDisplay) { - m_clusterLabelDisplay->setName("ClusterLabel"); - m_clusterLabelDisplay->setModel(model_); - m_clusterLabelDisplay->setParent(this); - addChild(m_clusterLabelDisplay); - m_clusterLabelDisplay->initialize(context_); - - // Make signal/slot connections - connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. CREATED"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); + Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); + if (m_clusterLabelDisplay = dynamic_cast(display); m_clusterLabelDisplay != nullptr) + { + m_clusterLabelDisplay->setName("ClusterLabel"); + m_clusterLabelDisplay->setModel(model_); + m_clusterLabelDisplay->setParent(this); + addChild(m_clusterLabelDisplay); + m_clusterLabelDisplay->initialize(context_); + + // Make signal/slot connections + connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. CREATED"); + } else { + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. NOT FOUND"); + } } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. NOT FOUND"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "rviz_mesh_tools_plugins/ClusterLabel. ALREADY EXISTING"); + m_clusterLabelDisplay->onEnable(); + m_clusterLabelDisplay->show(); + } +} + +void MapDisplay::disableClusterLabelDisplay() +{ + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->onDisable(); + m_clusterLabelDisplay->hide(); } +} - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh"); - Display* meshDisplay = createDisplay("rviz_mesh_tools_plugins/Mesh"); - if(m_meshDisplay = dynamic_cast(meshDisplay); m_meshDisplay != nullptr) +void MapDisplay::enableMeshDisplay() +{ + if(!m_meshDisplay) { - addChild(m_meshDisplay); - m_meshDisplay->setName("Mesh"); - m_meshDisplay->setModel(model_); - m_meshDisplay->setParent(this); - m_meshDisplay->initialize(context_); - m_meshDisplay->ignoreIncomingMessages(); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. CREATED"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh"); + Display* meshDisplay = createDisplay("rviz_mesh_tools_plugins/Mesh"); + if(m_meshDisplay = dynamic_cast(meshDisplay); m_meshDisplay != nullptr) + { + addChild(m_meshDisplay); + m_meshDisplay->setName("Mesh"); + m_meshDisplay->setModel(model_); + m_meshDisplay->setParent(this); + m_meshDisplay->initialize(context_); + m_meshDisplay->ignoreIncomingMessages(); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. CREATED"); + } else { + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. NOT FOUND"); + } } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. NOT FOUND"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "rviz_mesh_tools_plugins/Mesh. ALREADY EXISTING"); + m_meshDisplay->onEnable(); + m_meshDisplay->show(); } +} + +void MapDisplay::disableMeshDisplay() +{ + if(m_meshDisplay) + { + m_meshDisplay->onDisable(); + m_meshDisplay->hide(); + } +} + +void MapDisplay::onInitialize() +{ + std::string name = this->getName().toStdString(); + + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); + // enableClusterLabelDisplay(); + // enableMeshDisplay(); } void MapDisplay::onEnable() { - m_clusterLabelDisplay->onEnable(); - m_meshDisplay->onEnable(); + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->onEnable(); + } + + if(m_meshDisplay) + { + m_meshDisplay->onEnable(); + } } void MapDisplay::onDisable() { - m_clusterLabelDisplay->onDisable(); - m_meshDisplay->onDisable(); + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->onDisable(); + } + if(m_meshDisplay) + { + m_meshDisplay->onDisable(); + } + } // ===================================================================================================================== @@ -208,23 +265,31 @@ void MapDisplay::updateMap() return; } - // Update sub-plugins - m_meshDisplay->setGeometry(m_geometry); - m_meshDisplay->setVertexColors(m_colors); - m_meshDisplay->setVertexNormals(m_normals); - m_meshDisplay->clearVertexCosts(); - for (const auto& vertexCosts : m_costs) + if(m_meshDisplay) { - std::vector costs = vertexCosts.second; - m_meshDisplay->addVertexCosts(vertexCosts.first, costs); + // Update sub-plugins + m_meshDisplay->setGeometry(m_geometry); + m_meshDisplay->setVertexColors(m_colors); + m_meshDisplay->setVertexNormals(m_normals); + m_meshDisplay->clearVertexCosts(); + for (const auto& vertexCosts : m_costs) + { + std::vector costs = vertexCosts.second; + m_meshDisplay->addVertexCosts(vertexCosts.first, costs); + } + m_meshDisplay->setMaterials(m_materials, m_texCoords); + // m_meshDisplay->setTexCoords(m_texCoords); + for (uint32_t i = 0; i < m_textures.size(); i++) + { + m_meshDisplay->addTexture(m_textures[i], i); + } } - m_meshDisplay->setMaterials(m_materials, m_texCoords); - // m_meshDisplay->setTexCoords(m_texCoords); - for (uint32_t i = 0; i < m_textures.size(); i++) + + if(m_clusterLabelDisplay) { - m_meshDisplay->addTexture(m_textures[i], i); + m_clusterLabelDisplay->setData(m_geometry, m_clusterList); } - m_clusterLabelDisplay->setData(m_geometry, m_clusterList); + // All good setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); @@ -266,6 +331,9 @@ bool MapDisplay::loadData() { if (boost::filesystem::extension(mapFile).compare(".h5") == 0) { + enableClusterLabelDisplay(); // enable label writing to hdf5 + enableMeshDisplay(); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load HDF5 map"); // Open file IO hdf5_map_io::HDF5MapIO map_io(mapFile); @@ -392,9 +460,13 @@ bool MapDisplay::loadData() RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load channel " << costlayer << " as a costlayer!"); } } + + } else { + disableClusterLabelDisplay(); // we cannot write labels to standard formats + enableMeshDisplay(); std::cout << "LOADING WITH ASSIMP" << std::endl; // PLY, OBJ, DAE? -> ASSIMP // The following lines are a simple way to import the mesh geometry @@ -443,6 +515,8 @@ bool MapDisplay::loadData() } m_costs.clear(); + + } } catch (...) From 3ab0786673326f3bfebbd39256ee1e06919c9ce6 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 13 Nov 2023 17:42:03 +0100 Subject: [PATCH 31/40] supporting vertex colors for assimp loading --- .../include/rviz_mesh_tools_plugins/Types.hpp | 2 ++ rviz_mesh_tools_plugins/src/MapDisplay.cpp | 30 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp index 944ffa8..3e0149b 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp @@ -75,6 +75,8 @@ struct Normal } }; + + /// Struct for texture coordinates struct TexCoords { diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 52a8028..16e38fb 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -514,6 +514,36 @@ bool MapDisplay::loadData() m_geometry->faces[i].vertexIndices[2] = amesh->mFaces[i].mIndices[2]; } + if(amesh->HasNormals()) + { + m_normals.resize(amesh->mNumVertices, Normal(0.0, 0.0, 0.0)); + for(int i=0; imNumVertices; i++) + { + m_normals[i].x = amesh->mNormals[i].x; + m_normals[i].y = amesh->mNormals[i].y; + m_normals[i].z = amesh->mNormals[i].z; + } + } else { + m_normals.resize(0, Normal(0.0, 0.0, 0.0)); + } + + // assimp supports more color channels but not this plugin + // can we support this too? + int color_channel_id = 0; + if(amesh->HasVertexColors(color_channel_id)) + { + m_colors.resize(amesh->mNumVertices, Color(0.0, 0.0, 0.0, 0.0)); + for(int i=0; imNumVertices; i++) + { + m_colors[i].r = amesh->mColors[color_channel_id][i].r; + m_colors[i].g = amesh->mColors[color_channel_id][i].g; + m_colors[i].b = amesh->mColors[color_channel_id][i].b; + m_colors[i].a = amesh->mColors[color_channel_id][i].a; + } + } else { + m_colors.resize(0, Color(0.0, 0.0, 0.0, 0.0)); + } + m_costs.clear(); From 8e96a98135427c8164e85d8bfa77d2f6b800873b Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 00:25:46 +0100 Subject: [PATCH 32/40] mesh pose tool is working again. needed to replace raycasting operations because they are not available anymore. New functions using the depth buffer but its not very accurate. normal is post calculated by PCA. also not accurate. TODO: find better functions --- rviz_mesh_tools_plugins/CMakeLists.txt | 8 +- .../rviz_mesh_tools_plugins/MeshGoalTool.hpp | 22 +- .../rviz_mesh_tools_plugins/MeshPoseTool.hpp | 7 +- .../plugins_description.xml | 4 +- rviz_mesh_tools_plugins/src/MeshGoalTool.cpp | 47 +-- rviz_mesh_tools_plugins/src/MeshPoseTool.cpp | 294 +++++++++++++----- 6 files changed, 265 insertions(+), 117 deletions(-) diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index f5e6350..376597b 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -60,8 +60,8 @@ set(SOURCE_FILES src/MeshDisplay.cpp src/MeshVisual.cpp src/RvizFileProperty.cpp - # src/MeshPoseTool.cpp - # src/MeshGoalTool.cpp + src/MeshPoseTool.cpp + src/MeshGoalTool.cpp ) @@ -75,8 +75,8 @@ set(MOC_HEADER_FILES 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 + include/rviz_mesh_tools_plugins/MeshPoseTool.hpp + include/rviz_mesh_tools_plugins/MeshGoalTool.hpp ) # foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp index febcbf9..004a395 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp @@ -46,14 +46,16 @@ #define MESH_GOAL_TOOL_HPP #include -#include -#include -#include +#include +#include +#include #include #ifndef Q_MOC_RUN #include -#endif +#endif // Q_MOC_RUN + +#include namespace rviz_mesh_tools_plugins { @@ -91,15 +93,15 @@ private Q_SLOTS: virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); /// Property for the topic - rviz_common::StringProperty* topic_property_; + rviz_common::properties::StringProperty* topic_property_; /// Switch bottom / top for selection - rviz_common::BoolProperty* switch_bottom_top_; + rviz_common::properties::BoolProperty* switch_bottom_top_; /// Publisher - ros::Publisher pose_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; /// Node handle - ros::NodeHandle nh_; + // ros::NodeHandle nh_; }; -} /* namespace rviz_mesh_tools_plugins */ +} // namespace rviz_mesh_tools_plugins -#endif +#endif // MESH_GOAL_TOOL_HPP diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp index 0551a16..b71df18 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp @@ -93,8 +93,11 @@ class MeshPoseTool : public rviz_common::Tool Orientation }; State state_; - Ogre::Vector3 pos_; - Ogre::Vector3 ori_; + Ogre::Vector3 pos_start_; + Ogre::Vector3 normal_start_; + Ogre::Vector3 pos_last_; + // smooth normal in pixel coordinates > 2 + unsigned smooth_normal_width_ = 9; }; } /* namespace rviz_mesh_tools_plugins */ diff --git a/rviz_mesh_tools_plugins/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml index e401036..e87094d 100644 --- a/rviz_mesh_tools_plugins/plugins_description.xml +++ b/rviz_mesh_tools_plugins/plugins_description.xml @@ -34,11 +34,11 @@ Displays a mesh. - + diff --git a/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp b/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp index 3bbc23e..d77fc66 100644 --- a/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp @@ -45,37 +45,43 @@ #include "rviz_mesh_tools_plugins/MeshGoalTool.hpp" -#include +#include PLUGINLIB_EXPORT_CLASS( rviz_mesh_tools_plugins::MeshGoalTool, rviz_common::Tool ) -namespace rviz_mesh_tools_plugins{ +namespace rviz_mesh_tools_plugins +{ + MeshGoalTool::MeshGoalTool() { shortcut_key_ = 'm'; - topic_property_ = new rviz_common::StringProperty( "Topic", "goal", + topic_property_ = new rviz_common::properties::StringProperty( "Topic", "goal", "The topic on which to publish the mesh navigation goals.", getPropertyContainer(), SLOT(updateTopic()), this); - switch_bottom_top_ = new rviz_common::BoolProperty("Switch Bottom/Top", + switch_bottom_top_ = new rviz_common::properties::BoolProperty("Switch Bottom/Top", false, "Enable to stwich the bottom and top.", getPropertyContainer()); } - void MeshGoalTool::onInitialize() - { - MeshPoseTool::onInitialize(); - setName( "Mesh Goal" ); - updateTopic(); - } +void MeshGoalTool::onInitialize() +{ + MeshPoseTool::onInitialize(); + setName( "Mesh Goal" ); + updateTopic(); +} - void MeshGoalTool::updateTopic() - { - pose_pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); - } +void MeshGoalTool::updateTopic() +{ + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = node->create_publisher(topic_property_->getStdString(), 1); +} - void MeshGoalTool::onPoseSet( const Ogre::Vector3& position, const Ogre::Quaternion& orientation ){ - geometry_msgs::PoseStamped msg; +void MeshGoalTool::onPoseSet( + const Ogre::Vector3& position, + const Ogre::Quaternion& orientation ) +{ + geometry_msgs::msg::PoseStamped msg; msg.pose.position.x = position.x; msg.pose.position.y = position.y; msg.pose.position.z = position.z; @@ -106,9 +112,10 @@ MeshGoalTool::MeshGoalTool() msg.pose.orientation.z = ros_orientation.z; msg.pose.orientation.w = ros_orientation.w; - msg.header.stamp = ros::Time::now(); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + msg.header.stamp = node->now(); msg.header.frame_id = context_->getFixedFrame().toStdString(); - pose_pub_.publish(msg); - } - + pose_pub_->publish(msg); } + +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp index 27536e1..0753b60 100644 --- a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -48,6 +48,8 @@ #include #include #include +#include +#include #include #include @@ -58,9 +60,12 @@ #include #include #include +#include // method is private :( -#include +// #include +#include +#include #include @@ -71,28 +76,28 @@ namespace rviz_mesh_tools_plugins { -bool getViewportProjectionOnPlane( - rviz_rendering::RenderWindow* render_window, - int x, int y, - const Ogre::Plane& plane, - Ogre::Vector3& intersection_point) -{ - auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_window); - int width = viewport->getActualWidth(); - int height = viewport->getActualHeight(); - Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( - static_cast(x) / static_cast(width), - static_cast(y) / static_cast(height)); - - auto intersection = mouse_ray.intersects(plane); - if (!intersection.first) - { - return false; - } - - intersection_point = mouse_ray.getPoint(intersection.second); - return true; -} +// bool getViewportProjectionOnPlane( +// rviz_rendering::RenderWindow* render_window, +// int x, int y, +// const Ogre::Plane& plane, +// Ogre::Vector3& intersection_point) +// { +// auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_window); +// int width = viewport->getActualWidth(); +// int height = viewport->getActualHeight(); +// Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( +// static_cast(x) / static_cast(width), +// static_cast(y) / static_cast(height)); + +// auto intersection = mouse_ray.intersects(plane); +// if (!intersection.first) +// { +// return false; +// } + +// intersection_point = mouse_ray.getPoint(intersection.second); +// return true; +// } MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { @@ -131,46 +136,130 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) // RCLCPP_ASSERT(state_ == Position); assert(state_ == Position); - Ogre::Vector3 pos, ori; - if (selectTriangle(event, pos, ori)) + auto picker = context_->getViewPicker(); + + Ogre::Vector3 cur_pos; + if(picker->get3DPoint(event.panel, event.x, event.y, cur_pos)) { - pos_ = pos; - ori_ = ori; - arrow_->setPosition(pos_); - state_ = Orientation; - flags |= Render; + pos_start_ = cur_pos; + + + std::vector results; + unsigned patch_size = smooth_normal_width_; + bool success = picker->get3DPatch(event.panel, event.x, event.y, patch_size, patch_size, true, results); + + unsigned patch_middle = (patch_size * patch_size) / 2; + if(success && results.size() > 7) + { + // std::cout << "Found " << results.size() << " results" << std::endl; + + // PCA + // compute centroid + Ogre::Vector3 centroid(0.0, 0.0, 0.0); + for(size_t i=0; i(results.size()); + + Ogre::Matrix3 cov; + cov[0][0] = 0.0; cov[0][1] = 0.0; cov[0][2] = 0.0; + cov[1][0] = 0.0; cov[1][1] = 0.0; cov[1][2] = 0.0; + cov[2][0] = 0.0; cov[2][1] = 0.0; cov[2][2] = 0.0; + + for(size_t i=0; i(results.size())); + + float eig_vals[3]; + Ogre::Vector3 eig_vecs[3]; + cov.EigenSolveSymmetric(eig_vals, eig_vecs); + + // std::cout << "Eigen Analysis: " << std::endl; + + + // for(size_t i=0; i<3; i++) + // { + // std::cout << i << ". " << eig_vals[i] << std::endl; + // std::cout << eig_vecs[i] << std::endl; + // } + + normal_start_ = eig_vecs[2]; + pos_start_ = results[patch_middle]; + + normal_start_.normalise(); + + // flip normal to cam + + Ogre::Vector3 cam_pos = event.panel->getViewController()->getCamera()->getRealPosition(); + Ogre::Vector3 dir_to_cam = cam_pos - pos_start_; + dir_to_cam.normalise(); + + // there should be a function doing the dot product + float scalar = dir_to_cam.x * normal_start_.x + + dir_to_cam.y * normal_start_.y + + dir_to_cam.z * normal_start_.z; + + if(scalar < 0) + { + normal_start_ = -normal_start_; + } + + arrow_->setPosition(pos_start_); + state_ = Orientation; + } } + } else if (event.type == QEvent::MouseMove && event.left()) { if (state_ == Orientation) { - Ogre::Vector3 cur_pos; - Ogre::Plane plane(ori_, pos_); + auto picker = context_->getViewPicker(); - auto render_window = event.panel->getRenderWindow(); + Ogre::Vector3 cur_pos; + if(picker->get3DPoint(event.panel, event.x, event.y, cur_pos)) + { + // if valid override last pos + if( (cur_pos - pos_start_).squaredLength() > 0.001 * 0.001) + { + pos_last_ = cur_pos; + } - Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); + // if last pos is valid compute orientation + if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) + { + // compute orientation + Ogre::Vector3 dir_front = pos_last_ - pos_start_; + dir_front.normalise(); + // std::cout << "Dir Front: " << dir_front << std::endl; + Ogre::Vector3 dir_left = normal_start_.crossProduct(dir_front); + dir_left.normalise(); + // std::cout << "Dir Left: " << dir_left << std::endl; - if (getViewportProjectionOnPlane(render_window, event.x, event.y, plane, cur_pos)) - { - // right hand coordinate system - // x to the right, y to the top, and -z into the scene - // arrow foreward negative z - Ogre::Vector3 z_axis = -(cur_pos - pos_); - Ogre::Vector3 y_axis = ori_; - Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis); + Ogre::Vector3 dir_up = dir_front.crossProduct(dir_left); + dir_up.normalise(); + // std::cout << "Dir Up: " << dir_up << std::endl; - x_axis.normalise(); - y_axis.normalise(); - z_axis.normalise(); - arrow_->getSceneNode()->setVisible(true); - arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); + arrow_->getSceneNode()->setVisible(true); + arrow_->setOrientation(Ogre::Quaternion(dir_up, dir_left, -dir_front)); - flags |= Render; + flags |= Render; + } } } } @@ -178,33 +267,43 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) { if (state_ == Orientation) { - // rviz_rendering::project3DPointToViewportXY - Ogre::Vector3 cur_pos; - Ogre::Plane plane(ori_, pos_); - - // Start point on triangle is given. - // When releasing the left mouse button we search for - // the end point on the (infinite-sized) plane of the triangle - // - // ROS1: rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos) - // ROS2: ? - auto render_window = event.panel->getRenderWindow(); + auto picker = context_->getViewPicker(); - if (getViewportProjectionOnPlane(render_window, event.x, event.y, plane, cur_pos)) + Ogre::Vector3 cur_pos; + if(picker->get3DPoint(event.panel, event.x, event.y, cur_pos)) { - // arrow foreward negative z - Ogre::Vector3 z_axis = -(cur_pos - pos_); - Ogre::Vector3 y_axis = ori_; - Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis); - - x_axis.normalise(); - y_axis.normalise(); - z_axis.normalise(); - - onPoseSet(pos_, Ogre::Quaternion(x_axis, y_axis, z_axis)); + // if valid override last pos + if( (cur_pos - pos_start_).squaredLength() > 0.001 * 0.001) + { + pos_last_ = cur_pos; + } - flags |= (Finished | Render); + // if last pos is valid compute orientation + if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) + { + // compute orientation + Ogre::Vector3 dir_front = pos_last_ - pos_start_; + dir_front.normalise(); + // std::cout << "Dir Front: " << dir_front << std::endl; + + + Ogre::Vector3 dir_left = dir_front.crossProduct(normal_start_); + dir_left.normalise(); + // std::cout << "Dir Left: " << dir_left << std::endl; + + Ogre::Vector3 dir_up = dir_front.crossProduct(dir_left); + dir_up.normalise(); + // std::cout << "Dir Up: " << dir_up << std::endl; + + arrow_->getSceneNode()->setVisible(true); + arrow_->setOrientation(Ogre::Quaternion(dir_up, dir_left, -dir_front)); + + onPoseSet(pos_start_, Ogre::Quaternion(dir_up, dir_left, -dir_front)); + + flags |= (Finished | Render); + state_ = Position; + } } } } @@ -212,14 +311,50 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) return flags; } -bool MeshPoseTool::selectTriangle(rviz_common::ViewportMouseEvent& event, Ogre::Vector3& position, - Ogre::Vector3& triangle_normal) +bool MeshPoseTool::selectTriangle( + rviz_common::ViewportMouseEvent& event, + Ogre::Vector3& position, + Ogre::Vector3& triangle_normal) { - Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); + // Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( + // (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); + + // rviz_common::interaction::Picked pick_result; + + // auto manager = context_->getSelectionManager(); + + // bool hit = manager->pick( + // event.panel->getRenderWindow(), + // event.x, event.y, + // pick_result); + + // if(hit) + // { + // std::cout << "HIT" << std::endl; + // } else { + // std::cout << "NO HIT" << std::endl; + // } + + + auto picker = context_->getViewPicker(); + + Ogre::Vector3 int_point; + if(picker->get3DPoint(event.panel, event.x, event.y, int_point)) + { + std::cout << "HIT!" << std::endl; + } else { + std::cout << "NOT HIT :(" << std::endl; + } + + + + + Ogre::Ray ray; Ogre::RaySceneQuery* query = context_->getSceneManager()->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); + + query->setSortByDistance(true); Ogre::RaySceneQueryResult& result = query->execute(); @@ -241,8 +376,10 @@ bool MeshPoseTool::selectTriangle(rviz_common::ViewportMouseEvent& event, Ogre:: return false; } -bool MeshPoseTool::getPositionAndOrientation(const Ogre::ManualObject* mesh, const Ogre::Ray& ray, - Ogre::Vector3& position, Ogre::Vector3& orientation) +bool MeshPoseTool::getPositionAndOrientation( + const Ogre::ManualObject* mesh, + const Ogre::Ray& ray, + Ogre::Vector3& position, Ogre::Vector3& orientation) { Ogre::Real dist = -1.0f; Ogre::Vector3 a, b, c; @@ -338,7 +475,6 @@ void MeshPoseTool::getRawManualObjectData(const Ogre::ManualObject* mesh, const { index = static_cast(pLong[i]); } - else { index = static_cast(pShort[i]); From e8017edd73e39ce369c5ac5a79a741622365bd45 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 01:33:36 +0100 Subject: [PATCH 33/40] fixed mesh pose tool --- rviz_mesh_tools_plugins/CMakeLists.txt | 2 +- .../rviz_mesh_tools_plugins/MeshPoseTool.hpp | 2 +- rviz_mesh_tools_plugins/src/MeshPoseTool.cpp | 267 +++++------------- 3 files changed, 75 insertions(+), 196 deletions(-) diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 376597b..31afe08 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -41,7 +41,7 @@ include_directories(${ASSIMP_INCLUDE_DIRS}) # set(CMAKE_AUTOMOC ON) set(CMAKE_AUTOMOC ON) find_package(Qt5 COMPONENTS Core Widgets) -add_definitions(-DQT_NO_KEYWORDS) +# add_definitions(-DQT_NO_KEYWORDS) include_directories( include diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp index b71df18..4a29964 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp @@ -84,7 +84,7 @@ class MeshPoseTool : public rviz_common::Tool bool getPositionAndOrientation(const Ogre::ManualObject* mesh, const Ogre::Ray& ray, Ogre::Vector3& position, Ogre::Vector3& orientation); - bool selectTriangle(rviz_common::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& orientation); + bool selectTriangle(const Ogre::Ray& ray, Ogre::Vector3& position, Ogre::Vector3& orientation); rviz_rendering::Arrow* arrow_; enum State diff --git a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp index 0753b60..a98ffac 100644 --- a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -53,6 +53,7 @@ #include #include +#include "rviz_rendering/render_window.hpp" // #include "rviz_rendering/render_window.hpp" @@ -76,28 +77,17 @@ namespace rviz_mesh_tools_plugins { -// bool getViewportProjectionOnPlane( -// rviz_rendering::RenderWindow* render_window, -// int x, int y, -// const Ogre::Plane& plane, -// Ogre::Vector3& intersection_point) -// { -// auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_window); -// int width = viewport->getActualWidth(); -// int height = viewport->getActualHeight(); -// Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( -// static_cast(x) / static_cast(width), -// static_cast(y) / static_cast(height)); - -// auto intersection = mouse_ray.intersects(plane); -// if (!intersection.first) -// { -// return false; -// } - -// intersection_point = mouse_ray.getPoint(intersection.second); -// return true; -// } +Ogre::Ray getMouseEventRay( + const rviz_common::ViewportMouseEvent& event) +{ + auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()); + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( + static_cast(event.x) / static_cast(width), + static_cast(event.y) / static_cast(height)); + return mouse_ray; +} MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { @@ -136,170 +126,95 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) // RCLCPP_ASSERT(state_ == Position); assert(state_ == Position); - auto picker = context_->getViewPicker(); - - Ogre::Vector3 cur_pos; - if(picker->get3DPoint(event.panel, event.x, event.y, cur_pos)) + Ogre::Ray mouse_ray = getMouseEventRay(event); + + // Find intersection + Ogre::Vector3 int_pos; + Ogre::Vector3 int_normal; + if(selectTriangle(mouse_ray, int_pos, int_normal)) { - pos_start_ = cur_pos; - - - std::vector results; - unsigned patch_size = smooth_normal_width_; - bool success = picker->get3DPatch(event.panel, event.x, event.y, patch_size, patch_size, true, results); - - unsigned patch_middle = (patch_size * patch_size) / 2; - if(success && results.size() > 7) + pos_start_ = int_pos; + normal_start_ = int_normal; + + // Flip normal to camera + Ogre::Vector3 cam_pos = event.panel->getViewController()->getCamera()->getRealPosition(); + Ogre::Vector3 dir_to_cam = cam_pos - pos_start_; + dir_to_cam.normalise(); + // there should be a function doing the dot product + float scalar = dir_to_cam.x * normal_start_.x + + dir_to_cam.y * normal_start_.y + + dir_to_cam.z * normal_start_.z; + + if(scalar < 0) { - // std::cout << "Found " << results.size() << " results" << std::endl; - - // PCA - // compute centroid - Ogre::Vector3 centroid(0.0, 0.0, 0.0); - for(size_t i=0; i(results.size()); - - Ogre::Matrix3 cov; - cov[0][0] = 0.0; cov[0][1] = 0.0; cov[0][2] = 0.0; - cov[1][0] = 0.0; cov[1][1] = 0.0; cov[1][2] = 0.0; - cov[2][0] = 0.0; cov[2][1] = 0.0; cov[2][2] = 0.0; - - for(size_t i=0; i(results.size())); - - float eig_vals[3]; - Ogre::Vector3 eig_vecs[3]; - cov.EigenSolveSymmetric(eig_vals, eig_vecs); - - // std::cout << "Eigen Analysis: " << std::endl; - - - // for(size_t i=0; i<3; i++) - // { - // std::cout << i << ". " << eig_vals[i] << std::endl; - // std::cout << eig_vecs[i] << std::endl; - // } - - normal_start_ = eig_vecs[2]; - pos_start_ = results[patch_middle]; - - normal_start_.normalise(); - - // flip normal to cam - - Ogre::Vector3 cam_pos = event.panel->getViewController()->getCamera()->getRealPosition(); - Ogre::Vector3 dir_to_cam = cam_pos - pos_start_; - dir_to_cam.normalise(); - - // there should be a function doing the dot product - float scalar = dir_to_cam.x * normal_start_.x - + dir_to_cam.y * normal_start_.y - + dir_to_cam.z * normal_start_.z; - - if(scalar < 0) - { - normal_start_ = -normal_start_; - } - - arrow_->setPosition(pos_start_); - state_ = Orientation; + normal_start_ = -normal_start_; } - } + arrow_->setPosition(pos_start_); + state_ = Orientation; + } } else if (event.type == QEvent::MouseMove && event.left()) { if (state_ == Orientation) { - auto picker = context_->getViewPicker(); + Ogre::Plane plane(normal_start_, pos_start_); + Ogre::Ray mouse_ray = getMouseEventRay(event); - Ogre::Vector3 cur_pos; - if(picker->get3DPoint(event.panel, event.x, event.y, cur_pos)) + // intersect with plane + auto res = mouse_ray.intersects(plane); + if(res.first) { - // if valid override last pos - if( (cur_pos - pos_start_).squaredLength() > 0.001 * 0.001) - { - pos_last_ = cur_pos; - } - - // if last pos is valid compute orientation + // plane hit + pos_last_ = mouse_ray.getPoint(res.second); if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) { - // compute orientation - Ogre::Vector3 dir_front = pos_last_ - pos_start_; - dir_front.normalise(); - // std::cout << "Dir Front: " << dir_front << std::endl; - - Ogre::Vector3 dir_left = normal_start_.crossProduct(dir_front); - dir_left.normalise(); - // std::cout << "Dir Left: " << dir_left << std::endl; - - Ogre::Vector3 dir_up = dir_front.crossProduct(dir_left); - dir_up.normalise(); - // std::cout << "Dir Up: " << dir_up << std::endl; - - + // valid distance + + // right hand coordinate system + // x to the right, y to the top, and -z into the scene + // arrow foreward negative z + Ogre::Vector3 z_axis = -(pos_last_ - pos_start_).normalisedCopy(); + Ogre::Vector3 y_axis = normal_start_; + Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis).normalisedCopy(); + arrow_->getSceneNode()->setVisible(true); - arrow_->setOrientation(Ogre::Quaternion(dir_up, dir_left, -dir_front)); + arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); flags |= Render; - } + } } + } } else if (event.leftUp()) { if (state_ == Orientation) { + Ogre::Plane plane(normal_start_, pos_start_); + Ogre::Ray mouse_ray = getMouseEventRay(event); - auto picker = context_->getViewPicker(); - - Ogre::Vector3 cur_pos; - if(picker->get3DPoint(event.panel, event.x, event.y, cur_pos)) + // intersect with plane + auto res = mouse_ray.intersects(plane); + if(res.first) { - // if valid override last pos - if( (cur_pos - pos_start_).squaredLength() > 0.001 * 0.001) - { - pos_last_ = cur_pos; - } - - // if last pos is valid compute orientation + // plane hit + pos_last_ = mouse_ray.getPoint(res.second); if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) { - // compute orientation - Ogre::Vector3 dir_front = pos_last_ - pos_start_; - dir_front.normalise(); - // std::cout << "Dir Front: " << dir_front << std::endl; + // valid distance + + // right hand coordinate system + // x to the right, y to the top, and -z into the scene + // arrow foreward negative z + Ogre::Vector3 z_axis = -(pos_last_ - pos_start_).normalisedCopy(); + Ogre::Vector3 y_axis = normal_start_; + Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis).normalisedCopy(); - - Ogre::Vector3 dir_left = dir_front.crossProduct(normal_start_); - dir_left.normalise(); - // std::cout << "Dir Left: " << dir_left << std::endl; - - Ogre::Vector3 dir_up = dir_front.crossProduct(dir_left); - dir_up.normalise(); - // std::cout << "Dir Up: " << dir_up << std::endl; - arrow_->getSceneNode()->setVisible(true); - arrow_->setOrientation(Ogre::Quaternion(dir_up, dir_left, -dir_front)); - - onPoseSet(pos_start_, Ogre::Quaternion(dir_up, dir_left, -dir_front)); + arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); + + onPoseSet(pos_start_, Ogre::Quaternion(x_axis, y_axis, z_axis)); flags |= (Finished | Render); state_ = Position; @@ -312,49 +227,13 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) } bool MeshPoseTool::selectTriangle( - rviz_common::ViewportMouseEvent& event, + const Ogre::Ray& ray, Ogre::Vector3& position, Ogre::Vector3& triangle_normal) { - // Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - // (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); - - // rviz_common::interaction::Picked pick_result; - - // auto manager = context_->getSelectionManager(); - - // bool hit = manager->pick( - // event.panel->getRenderWindow(), - // event.x, event.y, - // pick_result); - - // if(hit) - // { - // std::cout << "HIT" << std::endl; - // } else { - // std::cout << "NO HIT" << std::endl; - // } - - - auto picker = context_->getViewPicker(); - - Ogre::Vector3 int_point; - if(picker->get3DPoint(event.panel, event.x, event.y, int_point)) - { - std::cout << "HIT!" << std::endl; - } else { - std::cout << "NOT HIT :(" << std::endl; - } - - - - - Ogre::Ray ray; - Ogre::RaySceneQuery* query = context_->getSceneManager()->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); - query->setSortByDistance(true); Ogre::RaySceneQueryResult& result = query->execute(); From 1d1969a38bea04392d985fd1a3b15748c76339a0 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 01:39:54 +0100 Subject: [PATCH 34/40] replaced assimp with rviz_assimp_vendor --- rviz_mesh_tools_plugins/CMakeLists.txt | 8 +++++--- rviz_mesh_tools_plugins/package.xml | 1 + 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 31afe08..be0687f 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(rmw REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) find_package(rviz_ogre_vendor REQUIRED) -find_package(rviz_default_plugins REQUIRED) +find_package(rviz_assimp_vendor REQUIRED) find_package(std_msgs REQUIRED) find_package(hdf5_map_io REQUIRED) find_package(mesh_msgs REQUIRED) @@ -30,12 +30,12 @@ find_package(std_msgs REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem) find_package(HDF5 REQUIRED COMPONENTS C CXX HL) -find_package(assimp REQUIRED) +# find_package(assimp REQUIRED) # This needs to be optional for non CL devices find_package(OpenCL 2 REQUIRED) -include_directories(${ASSIMP_INCLUDE_DIRS}) +# include_directories(${ASSIMP_INCLUDE_DIRS}) ## This setting causes Qt's "MOC" generation to happen automatically. # set(CMAKE_AUTOMOC ON) @@ -122,6 +122,7 @@ ament_target_dependencies(${PROJECT_NAME} rmw rviz_common rviz_rendering + rviz_assimp_vendor std_msgs hdf5_map_io mesh_msgs @@ -142,6 +143,7 @@ ament_export_dependencies( rmw rviz_common rviz_rendering + rviz_assimp_vendor std_msgs hdf5_map_io mesh_msgs diff --git a/rviz_mesh_tools_plugins/package.xml b/rviz_mesh_tools_plugins/package.xml index c9b54c9..cf70504 100644 --- a/rviz_mesh_tools_plugins/package.xml +++ b/rviz_mesh_tools_plugins/package.xml @@ -24,6 +24,7 @@ rclcpp rviz_common rviz_rendering + rviz_assimp_vendor std_msgs mesh_msgs hdf5_map_io From a6c8f7a4df13000dabc2c81b6c511f12abedfdf1 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 12:30:37 +0100 Subject: [PATCH 35/40] moved raycasting operations to one file. better reusage --- rviz_mesh_tools_plugins/CMakeLists.txt | 2 + .../InteractionHelper.hpp | 41 ++++ .../rviz_mesh_tools_plugins/MeshPoseTool.hpp | 8 - .../src/ClusterLabelTool.cpp | 34 +++- .../src/InteractionHelper.cpp | 176 ++++++++++++++++++ rviz_mesh_tools_plugins/src/MeshPoseTool.cpp | 158 +--------------- 6 files changed, 250 insertions(+), 169 deletions(-) create mode 100644 rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp create mode 100644 rviz_mesh_tools_plugins/src/InteractionHelper.cpp diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index be0687f..17d3a43 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -62,6 +62,7 @@ set(SOURCE_FILES src/RvizFileProperty.cpp src/MeshPoseTool.cpp src/MeshGoalTool.cpp + src/InteractionHelper.cpp ) @@ -77,6 +78,7 @@ set(MOC_HEADER_FILES include/rviz_mesh_tools_plugins/RvizFileProperty.hpp include/rviz_mesh_tools_plugins/MeshPoseTool.hpp include/rviz_mesh_tools_plugins/MeshGoalTool.hpp + include/rviz_mesh_tools_plugins/InteractionHelper.hpp ) # foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp new file mode 100644 index 0000000..55203b0 --- /dev/null +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp @@ -0,0 +1,41 @@ +#ifndef RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP +#define RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP + +#include +#include + + +namespace rviz_common +{ + class DisplayContext; +} // namespace rviz_common + +namespace rviz_mesh_tools_plugins +{ + +struct Intersection { + double range; + Ogre::Vector3 point; + Ogre::Vector3 normal; + uint32_t face_id; + uint32_t geom_id; // currently unused + uint32_t inst_id; // currently unused +}; + +Ogre::Ray getMouseEventRay( + const rviz_common::ViewportMouseEvent& event); + +bool selectFace( + const Ogre::ManualObject* mesh, + const Ogre::Ray& ray, + Intersection& intersection); + +bool selectFace( + const rviz_common::DisplayContext* ctx, + const Ogre::Ray& ray, + Intersection& intersection); + +} // namespace rviz_mesh_tools_plugins + + +#endif // RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp index 4a29964..751229a 100644 --- a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp @@ -78,14 +78,6 @@ class MeshPoseTool : public rviz_common::Tool protected: virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) = 0; - void getRawManualObjectData(const Ogre::ManualObject* mesh, const size_t sectionNumber, size_t& vertexCount, - Ogre::Vector3*& vertices, size_t& indexCount, unsigned long*& indices); - - bool getPositionAndOrientation(const Ogre::ManualObject* mesh, const Ogre::Ray& ray, Ogre::Vector3& position, - Ogre::Vector3& orientation); - - bool selectTriangle(const Ogre::Ray& ray, Ogre::Vector3& position, Ogre::Vector3& orientation); - rviz_rendering::Arrow* arrow_; enum State { diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 61dcdcd..14dd104 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -50,6 +50,8 @@ #include #include +#include + #include #include #include @@ -64,9 +66,12 @@ #include +#include + #include #include #include +#include #include @@ -79,6 +84,8 @@ namespace rviz_mesh_tools_plugins { #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" + + ClusterLabelTool::ClusterLabelTool() :rviz_common::Tool() ,m_displayInitialized(false) @@ -86,9 +93,6 @@ ClusterLabelTool::ClusterLabelTool() // ,m_evt_stop(nullptr, (QMouseEvent*)nullptr, 0, 0) { shortcut_key_ = 'l'; - - // ros::NodeHandle n; - // m_labelPublisher = n.advertise("/cluster_label", 1, true); } ClusterLabelTool::~ClusterLabelTool() @@ -130,7 +134,7 @@ void ClusterLabelTool::onInitialize() // std::cout << "ClusterLabelTool INIT kdwioh" << std::endl; - // try-catch block to check for OpenCL errors + // // try-catch block to check for OpenCL errors // try // { // // Initialize OpenCL @@ -358,13 +362,26 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) void ClusterLabelTool::updateSelectionBox() { // TODO CHECK THIS - float left, right, top, bottom; // left = m_evt_start.x * 2 - 1; // right = m_evt_stop.x * 2 - 1; // top = 1 - m_evt_start.y * 2; // bottom = 1 - m_evt_stop.y * 2; + auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(m_evt_panel->getRenderWindow()); + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + + float m_bb_x1_rel = static_cast(m_bb_x1) / static_cast(width); + float m_bb_x2_rel = static_cast(m_bb_x2) / static_cast(width); + float m_bb_y1_rel = static_cast(m_bb_y1) / static_cast(height); + float m_bb_y2_rel = static_cast(m_bb_y2) / static_cast(height); + + float left = m_bb_x1_rel * 2.0 - 1.0; + float right = m_bb_x2_rel * 2.0 - 1.0; + float top = 1.0 - m_bb_y1_rel * 2.0; + float bottom = 1.0 - m_bb_y2_rel * 2.0; + m_selectionBox->clear(); m_selectionBox->begin(m_selectionBoxMaterial->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); m_selectionBox->position(left, top, -1); @@ -549,15 +566,16 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume } } -void ClusterLabelTool::selectSingleFace(rviz_common::ViewportMouseEvent& event, bool selectMode) +void ClusterLabelTool::selectSingleFace( + 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() // ); - Ogre::Ray ray; - throw std::runtime_error("TODO"); + Ogre::Ray ray = getMouseEventRay(event); selectSingleFaceParallel(ray, selectMode); } diff --git a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp new file mode 100644 index 0000000..716473d --- /dev/null +++ b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp @@ -0,0 +1,176 @@ +#include + +#include +#include +#include + +#include +#include +#include + +namespace rviz_mesh_tools_plugins +{ + +Ogre::Ray getMouseEventRay( + const rviz_common::ViewportMouseEvent& event) +{ + auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()); + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( + static_cast(event.x) / static_cast(width), + static_cast(event.y) / static_cast(height)); + return mouse_ray; +} + + +void getRawManualObjectData( + const Ogre::ManualObject* mesh, + const size_t sectionNumber, + size_t& vertexCount, Ogre::Vector3*& vertices, + size_t& indexCount, unsigned long*& indices) +{ + Ogre::VertexData* vertexData; + const Ogre::VertexElement* vertexElement; + Ogre::HardwareVertexBufferSharedPtr vertexBuffer; + unsigned char* vertexChar; + float* vertexFloat; + + vertexData = mesh->getSection(sectionNumber)->getRenderOperation()->vertexData; + vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION); + vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource()); + vertexChar = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); + + vertexCount = vertexData->vertexCount; + vertices = new Ogre::Vector3[vertexCount]; + + for (size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize()) + { + vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat); + vertices[i] = + (mesh->getParentNode()->_getDerivedOrientation() * + (Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]) * mesh->getParentNode()->_getDerivedScale())) + + mesh->getParentNode()->_getDerivedPosition(); + } + + vertexBuffer->unlock(); + + Ogre::IndexData* indexData; + Ogre::HardwareIndexBufferSharedPtr indexBuffer; + indexData = mesh->getSection(sectionNumber)->getRenderOperation()->indexData; + indexCount = indexData->indexCount; + indices = new unsigned long[indexCount]; + indexBuffer = indexData->indexBuffer; + unsigned int* pLong = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); + unsigned short* pShort = reinterpret_cast(pLong); + + for (size_t i = 0; i < indexCount; i++) + { + unsigned long index; + if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT) + { + index = static_cast(pLong[i]); + } + else + { + index = static_cast(pShort[i]); + } + + indices[i] = index; + } + indexBuffer->unlock(); +} + +bool selectFace( + const Ogre::ManualObject* mesh, + const Ogre::Ray& ray, + Intersection& intersection) +{ + Ogre::Real dist = -1.0f; + Ogre::Vector3 a, b, c; + + size_t vertex_count = 0; + Ogre::Vector3* vertices; + size_t index_count = 0; + unsigned long* indices; + size_t num_sections = mesh->getNumSections(); + + for (size_t i = 0; i < num_sections; i++) + { + getRawManualObjectData(mesh, i, vertex_count, vertices, index_count, indices); + if (index_count != 0) + { + for (size_t j = 0; j < index_count; j += 3) + { + std::pair goal = Ogre::Math::intersects(ray, vertices[indices[j]], vertices[indices[j + 1]], + vertices[indices[j + 2]], true, true); + + if (goal.first) + { + if ((dist < 0.0f) || (goal.second < dist)) + { + dist = goal.second; + a = vertices[indices[j]]; + b = vertices[indices[j + 1]]; + c = vertices[indices[j + 2]]; + } + } + } + } + } + + delete[] vertices; + delete[] indices; + if (dist != -1) + { + intersection.range = dist; + intersection.point = ray.getPoint(dist); + Ogre::Vector3 ab = b - a; + Ogre::Vector3 ac = c - a; + intersection.normal = ac.crossProduct(ab).normalisedCopy(); + return true; + } + else + { + return false; + } +} + + +bool selectFace( + Ogre::SceneManager* sm, + const Ogre::Ray& ray, + Intersection& intersection) +{ + Ogre::RaySceneQuery* query = sm->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); + query->setSortByDistance(true); + + Ogre::RaySceneQueryResult& result = query->execute(); + + for (size_t i = 0; i < result.size(); i++) + { + if (result[i].movable->getName().find("TriangleMesh") != std::string::npos) + { + Ogre::ManualObject* mesh = static_cast(result[i].movable); + size_t goal_section = -1; + size_t goal_index = -1; + Ogre::Real dist = -1; + if (selectFace(mesh, ray, intersection)) + { + return true; + } + } + } + return false; +} + + +bool selectFace( + const rviz_common::DisplayContext* ctx, + const Ogre::Ray& ray, + Intersection& intersection) +{ + return selectFace(ctx->getSceneManager(), ray, intersection); +} + +} // namespace rviz_mesh_tools_plugins \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp index a98ffac..2f35cdf 100644 --- a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -72,23 +72,12 @@ #include "rviz_mesh_tools_plugins/MeshPoseTool.hpp" +#include namespace rviz_mesh_tools_plugins { -Ogre::Ray getMouseEventRay( - const rviz_common::ViewportMouseEvent& event) -{ - auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()); - int width = viewport->getActualWidth(); - int height = viewport->getActualHeight(); - Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( - static_cast(event.x) / static_cast(width), - static_cast(event.y) / static_cast(height)); - return mouse_ray; -} - MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) { } @@ -131,10 +120,11 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) // Find intersection Ogre::Vector3 int_pos; Ogre::Vector3 int_normal; - if(selectTriangle(mouse_ray, int_pos, int_normal)) + Intersection intersection; + if(selectFace(context_, mouse_ray, intersection)) { - pos_start_ = int_pos; - normal_start_ = int_normal; + pos_start_ = intersection.point; + normal_start_ = intersection.normal; // Flip normal to camera Ogre::Vector3 cam_pos = event.panel->getViewController()->getCamera()->getRealPosition(); @@ -226,142 +216,4 @@ int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) return flags; } -bool MeshPoseTool::selectTriangle( - const Ogre::Ray& ray, - Ogre::Vector3& position, - Ogre::Vector3& triangle_normal) -{ - Ogre::RaySceneQuery* query = - context_->getSceneManager()->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); - - query->setSortByDistance(true); - - Ogre::RaySceneQueryResult& result = query->execute(); - - for (size_t i = 0; i < result.size(); i++) - { - if (result[i].movable->getName().find("TriangleMesh") != std::string::npos) - { - Ogre::ManualObject* mesh = static_cast(result[i].movable); - size_t goal_section = -1; - size_t goal_index = -1; - Ogre::Real dist = -1; - if (getPositionAndOrientation(mesh, ray, position, triangle_normal)) - { - return true; - } - } - } - return false; -} - -bool MeshPoseTool::getPositionAndOrientation( - const Ogre::ManualObject* mesh, - const Ogre::Ray& ray, - Ogre::Vector3& position, Ogre::Vector3& orientation) -{ - Ogre::Real dist = -1.0f; - Ogre::Vector3 a, b, c; - - size_t vertex_count = 0; - Ogre::Vector3* vertices; - size_t index_count = 0; - unsigned long* indices; - size_t num_sections = mesh->getNumSections(); - - for (size_t i = 0; i < num_sections; i++) - { - getRawManualObjectData(mesh, i, vertex_count, vertices, index_count, indices); - if (index_count != 0) - { - for (size_t j = 0; j < index_count; j += 3) - { - std::pair goal = Ogre::Math::intersects(ray, vertices[indices[j]], vertices[indices[j + 1]], - vertices[indices[j + 2]], true, true); - - if (goal.first) - { - if ((dist < 0.0f) || (goal.second < dist)) - { - dist = goal.second; - a = vertices[indices[j]]; - b = vertices[indices[j + 1]]; - c = vertices[indices[j + 2]]; - } - } - } - } - } - - delete[] vertices; - delete[] indices; - if (dist != -1) - { - position = ray.getPoint(dist); - Ogre::Vector3 ab = b - a; - Ogre::Vector3 ac = c - a; - orientation = ac.crossProduct(ab).normalisedCopy(); - return true; - } - else - { - return false; - } -} - -void MeshPoseTool::getRawManualObjectData(const Ogre::ManualObject* mesh, const size_t sectionNumber, - size_t& vertexCount, Ogre::Vector3*& vertices, size_t& indexCount, - unsigned long*& indices) -{ - Ogre::VertexData* vertexData; - const Ogre::VertexElement* vertexElement; - Ogre::HardwareVertexBufferSharedPtr vertexBuffer; - unsigned char* vertexChar; - float* vertexFloat; - - vertexData = mesh->getSection(sectionNumber)->getRenderOperation()->vertexData; - vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION); - vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource()); - vertexChar = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); - - vertexCount = vertexData->vertexCount; - vertices = new Ogre::Vector3[vertexCount]; - - for (size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize()) - { - vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat); - vertices[i] = - (mesh->getParentNode()->_getDerivedOrientation() * - (Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]) * mesh->getParentNode()->_getDerivedScale())) + - mesh->getParentNode()->_getDerivedPosition(); - } - - vertexBuffer->unlock(); - - Ogre::IndexData* indexData; - Ogre::HardwareIndexBufferSharedPtr indexBuffer; - indexData = mesh->getSection(sectionNumber)->getRenderOperation()->indexData; - indexCount = indexData->indexCount; - indices = new unsigned long[indexCount]; - indexBuffer = indexData->indexBuffer; - unsigned int* pLong = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); - unsigned short* pShort = reinterpret_cast(pLong); - - for (size_t i = 0; i < indexCount; i++) - { - unsigned long index; - if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT) - { - index = static_cast(pLong[i]); - } - else - { - index = static_cast(pShort[i]); - } - - indices[i] = index; - } - indexBuffer->unlock(); -} - } // namespace rviz_mesh_tools_plugins From ea0a9100180adb60d0549cc5f4a9892e455437b5 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 14:01:04 +0100 Subject: [PATCH 36/40] fixes. storing rviz config is not working --- .../src/ClusterLabelDisplay.cpp | 18 +-- .../src/ClusterLabelTool.cpp | 138 +++++++++--------- .../src/ClusterLabelVisual.cpp | 12 +- .../src/InteractionHelper.cpp | 1 - rviz_mesh_tools_plugins/src/MapDisplay.cpp | 18 +-- rviz_mesh_tools_plugins/src/MeshPoseTool.cpp | 9 +- 6 files changed, 107 insertions(+), 89 deletions(-) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index bda55dd..10027b0 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -126,7 +126,7 @@ std::shared_ptr ClusterLabelDisplay::getGeometry() { if (!m_geometry) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Geometry requested, but none available!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Geometry requested, but none available!"); } return m_geometry; } @@ -135,7 +135,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector { if (has_data) { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: already has data, but setData() was called again!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: already has data, but setData() was called again!"); } // Copy data @@ -144,7 +144,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector m_clusterList.insert(m_clusterList.begin(), Cluster("__NEW__", vector())); // Set flag - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: received data"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: received data"); has_data = true; // Draw visuals @@ -184,11 +184,11 @@ void ClusterLabelDisplay::changeVisual() { if (m_activeVisualProperty->getStdString().empty()) { - RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Should change visual but no visual selected!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Should change visual but no visual selected!"); return; } - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); m_activeVisualId = m_activeVisualProperty->getOptionInt(); @@ -198,11 +198,11 @@ void ClusterLabelDisplay::changeVisual() void ClusterLabelDisplay::updateMap() { - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Update"); if (!has_data) { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: No data available! Can't show map"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: No data available! Can't show map"); return; } @@ -297,7 +297,7 @@ void ClusterLabelDisplay::createVisualsFromClusterList() ss << "ClusterLabelVisual_" << i; auto visual = std::make_shared(context_, ss.str(), m_geometry); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Label Display: Create visual for label '" << m_clusterList[i].name << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Create visual for label '" << m_clusterList[i].name << "'"); visual->setFacesInCluster(m_clusterList[i].faces); visual->setColor(getRainbowColor((++colorIndex / m_clusterList.size())), m_alphaProperty->getFloat()); m_visuals.push_back(visual); @@ -358,7 +358,7 @@ void ClusterLabelDisplay::notifyLabelTool() void ClusterLabelDisplay::addLabel(std::string label, std::vector faces) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Cluster Label Display: add label '" << label << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Display: add label '" << label << "'"); Q_EMIT signalAddLabel(Cluster(label, faces)); } diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index 14dd104..eef383f 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -107,7 +107,7 @@ ClusterLabelTool::~ClusterLabelTool() // context_ are set. It should be called only once per instantiation. void ClusterLabelTool::onInitialize() { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "ClusterLabelTool: Call Init"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelTool: Call Init"); auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); m_labelPublisher = node->create_publisher( @@ -122,9 +122,15 @@ void ClusterLabelTool::onInitialize() m_selectionBox->setQueryFlags(0); m_sceneNode->attachObject(m_selectionBox); - m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().create( - "ClusterLabelTool_SelectionBoxMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); + m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().getByName( + "ClusterLabelTool_SelectionBoxMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + if(!m_selectionBoxMaterial) + { + m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().create( + "ClusterLabelTool_SelectionBoxMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); + } + m_selectionBoxMaterial->setAmbient(Ogre::ColourValue(0, 0, 255, 0.5)); m_selectionBoxMaterial->setDiffuse(0, 0, 0, 0.5); m_selectionBoxMaterial->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); @@ -138,28 +144,28 @@ void ClusterLabelTool::onInitialize() // try // { // // Initialize OpenCL - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Get platforms"); + // 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_plugin"), "Found platform: %s", platform.getInfo().c_str()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "platform version: %s", platform.getInfo().c_str()); + // 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_plugin"), " "); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); // vector consideredDevices; // for (auto const& platform : platforms) // { - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Get devices of %s: ", platform.getInfo().c_str()); + // 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_plugin"), "Found device: %s", device.getInfo().c_str()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Device work units: %d", device.getInfo()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Device work group size: %lu", device.getInfo()); + // 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 @@ -171,7 +177,7 @@ void ClusterLabelTool::onInitialize() // // use bitwise AND to extract the number in the lower 16 bits // cl_uint minorVersion = version & 0x0000FFFF; - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_plugin"), "Found a device with OpenCL version: %u.%u", majorVersion, minorVersion); + // 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) @@ -181,7 +187,7 @@ void ClusterLabelTool::onInitialize() // } // } // } - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), " "); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); // cl::Platform platform; // // Preferably choose the first compatible device of type GPU @@ -206,7 +212,7 @@ void ClusterLabelTool::onInitialize() // if (!deviceFound) // { // // Panic if no compatible device was found - // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "No device with compatible OpenCL version found (minimum 2.0)"); + // 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(); // } @@ -214,9 +220,9 @@ void ClusterLabelTool::onInitialize() // 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_plugin"), "Using device %s of platform %s", m_clDevice.getInfo().c_str(), + // 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_plugin"), " "); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); // // Read kernel file // // may throw ament_index_cpp::PackageNotFoundError exception @@ -226,7 +232,7 @@ void ClusterLabelTool::onInitialize() // std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "Got kernel: %s%s", package_share_directory.c_str(), CL_RAY_CAST_KERNEL_FILE); + // 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() }); @@ -234,12 +240,12 @@ void ClusterLabelTool::onInitialize() // try // { // m_clProgram.build({ m_clDevice }); - // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_plugin"), "Successfully built program."); + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successfully built program."); // } // catch (cl::Error& err) // { - // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_plugin"), "Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); + // 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(); @@ -251,8 +257,8 @@ void ClusterLabelTool::onInitialize() // } // catch (cl::Error err) // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.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(); @@ -350,8 +356,8 @@ void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) // } // catch (cl::Error err) // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.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(); @@ -391,6 +397,8 @@ void ClusterLabelTool::updateSelectionBox() m_selectionBox->triangle(0, 1, 2); m_selectionBox->triangle(0, 2, 3); m_selectionBox->end(); + + } void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) @@ -403,12 +411,10 @@ void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) // 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; - m_bb_x2 = m_bb_x1; m_bb_y2 = m_bb_y1; m_selectionBox->clear(); @@ -433,7 +439,8 @@ void ClusterLabelTool::selectionBoxMove(rviz_common::ViewportMouseEvent& event) } void ClusterLabelTool::selectMultipleFaces( - rviz_common::ViewportMouseEvent& event, bool selectMode) + rviz_common::ViewportMouseEvent& event, + bool selectMode) { m_selectionBox->setVisible(false); @@ -458,8 +465,36 @@ void ClusterLabelTool::selectMultipleFaces( const int BOX_SIZE_TOLERANCE = 1; if ((right - left) * (bottom - top) < BOX_SIZE_TOLERANCE) { + // single face selectSingleFace(event, selectMode); - return; + } else { + rviz_common::interaction::M_Picked pick_results; + + auto manager = context_->getSelectionManager(); + + manager->pick( + m_evt_panel->getRenderWindow(), + m_bb_x1, m_bb_y1, + m_bb_x2, m_bb_y2, + pick_results); + + if(!pick_results.empty()) + { + // FOUND SOMETHING! + std::cout << "FOUND SOMETHING! " << pick_results.size() << std::endl; + + 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; + + } + + // TODO: continue implementing this + } } // Ogre::PlaneBoundedVolume volume = @@ -477,34 +512,6 @@ void ClusterLabelTool::selectMultipleFaces( // int y2, // M_Picked & results) = 0; - rviz_common::interaction::M_Picked pick_results; - - auto manager = context_->getSelectionManager(); - - manager->pick( - m_evt_panel->getRenderWindow(), - m_bb_x1, m_bb_y1, - m_bb_x2, m_bb_y2, - pick_results); - - if(!pick_results.empty()) - { - // FOUND SOMETHING! - std::cout << "FOUND SOMETHING!" << std::endl; - - for(auto elem : pick_results) - { - rviz_common::interaction::CollObjectHandle key = elem.first; - rviz_common::interaction::Picked value = elem.second; - - std::cout << "Extra handles: " << value.extra_handles.size() << std::endl; - - - } - - - // TODO: continue implementing this - } } @@ -533,8 +540,8 @@ void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume // } // catch (cl::Error err) // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.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++) @@ -600,8 +607,8 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) // } // catch (cl::Error err) // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.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; @@ -636,7 +643,7 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) m_visual->setFacesInCluster(tmpFaceList); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "selectSingleFaceParallel() found face with id %d", closestFaceId); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "selectSingleFaceParallel() found face with id %d", closestFaceId); } } @@ -679,8 +686,8 @@ void ClusterLabelTool::selectSphereFacesParallel( // } // catch (cl::Error err) // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.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++) @@ -732,8 +739,8 @@ boost::optional> ClusterLabelTool::getClosestIntersec // } // catch (cl::Error err) // { - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), err.what() << ": " << CLUtil::getErrorString(err.err())); - // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "(" << CLUtil::getErrorDescription(err.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; @@ -762,7 +769,7 @@ boost::optional> ClusterLabelTool::getClosestIntersec void ClusterLabelTool::publishLabel(std::string label) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_plugin"), "Label Tool: Publish label '" << 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++) @@ -804,6 +811,7 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) } else if (event.leftUp() && m_multipleSelect) { + std::cout << "BLA" << std::endl; m_multipleSelect = false; selectMultipleFaces(event, true); } diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index aef7363..a96fbee 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -139,7 +139,7 @@ ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std } // Create a submesh and a custom material for it - if (!m_mesh.isNull()) + if(!m_mesh.isNull()) { m_subMesh = m_mesh->createSubMesh(m_labelId); m_subMesh->useSharedVertices = true; @@ -185,10 +185,16 @@ ClusterLabelVisual::~ClusterLabelVisual() void ClusterLabelVisual::reset() { + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Reset ClusterLabelVisual"); if (!m_material.isNull()) { - Ogre::MaterialManager::getSingleton().unload(m_material->getName()); - Ogre::MaterialManager::getSingleton().remove(m_material->getName()); + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << m_material->getName() << "' to unload. skipping"); + } } } diff --git a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp index 716473d..05e0172 100644 --- a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp +++ b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp @@ -136,7 +136,6 @@ bool selectFace( } } - bool selectFace( Ogre::SceneManager* sm, const Ogre::Ray& ray, diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 16e38fb..85d45f2 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -120,7 +120,7 @@ void MapDisplay::enableClusterLabelDisplay() { if(!m_clusterLabelDisplay) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); if (m_clusterLabelDisplay = dynamic_cast(display); m_clusterLabelDisplay != nullptr) { @@ -132,12 +132,12 @@ void MapDisplay::enableClusterLabelDisplay() // Make signal/slot connections connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. CREATED"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. CREATED"); } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. NOT FOUND"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. NOT FOUND"); } } else { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "rviz_mesh_tools_plugins/ClusterLabel. ALREADY EXISTING"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "rviz_mesh_tools_plugins/ClusterLabel. ALREADY EXISTING"); m_clusterLabelDisplay->onEnable(); m_clusterLabelDisplay->show(); } @@ -156,7 +156,7 @@ void MapDisplay::enableMeshDisplay() { if(!m_meshDisplay) { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/Mesh"); Display* meshDisplay = createDisplay("rviz_mesh_tools_plugins/Mesh"); if(m_meshDisplay = dynamic_cast(meshDisplay); m_meshDisplay != nullptr) { @@ -166,12 +166,12 @@ void MapDisplay::enableMeshDisplay() m_meshDisplay->setParent(this); m_meshDisplay->initialize(context_); m_meshDisplay->ignoreIncomingMessages(); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. CREATED"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/Mesh. CREATED"); } else { - RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/Mesh. NOT FOUND"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/Mesh. NOT FOUND"); } } else { - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "rviz_mesh_tools_plugins/Mesh. ALREADY EXISTING"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "rviz_mesh_tools_plugins/Mesh. ALREADY EXISTING"); m_meshDisplay->onEnable(); m_meshDisplay->show(); } @@ -190,7 +190,7 @@ void MapDisplay::onInitialize() { std::string name = this->getName().toStdString(); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_plugin"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); // enableClusterLabelDisplay(); // enableMeshDisplay(); diff --git a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp index 2f35cdf..7b19b8a 100644 --- a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -78,13 +78,18 @@ namespace rviz_mesh_tools_plugins { -MeshPoseTool::MeshPoseTool() : rviz_common::Tool(), arrow_(NULL) +MeshPoseTool::MeshPoseTool() +:rviz_common::Tool() +,arrow_(NULL) { } MeshPoseTool::~MeshPoseTool() { - delete arrow_; + if(arrow_) + { + delete arrow_; + } } void MeshPoseTool::onInitialize() From 65a8736c40c1486d2670f57c9be31c434459eea1 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 15:06:33 +0100 Subject: [PATCH 37/40] ported mesh_msgs_transform --- mesh_msgs_transform/CMakeLists.txt | 67 +++++++++++----- mesh_msgs_transform/COLCON_IGNORE | 0 .../include/mesh_msgs_transform/transforms.h | 27 ++++--- mesh_msgs_transform/package.xml | 25 +++--- mesh_msgs_transform/src/transforms.cpp | 80 ++++++++++--------- 5 files changed, 116 insertions(+), 83 deletions(-) delete mode 100644 mesh_msgs_transform/COLCON_IGNORE diff --git a/mesh_msgs_transform/CMakeLists.txt b/mesh_msgs_transform/CMakeLists.txt index f1681df..5dc90b8 100644 --- a/mesh_msgs_transform/CMakeLists.txt +++ b/mesh_msgs_transform/CMakeLists.txt @@ -12,41 +12,66 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - tf - geometry_msgs - mesh_msgs -) - +find_package(mesh_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) -catkin_package( - INCLUDE_DIRS include - LIBRARIES mesh_msgs_transform - CATKIN_DEPENDS tf geometry_msgs mesh_msgs +add_library(${PROJECT_NAME} + src/transforms.cpp ) -add_library(mesh_msgs_transform - src/transforms.cpp +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" + ${EIGEN3_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + Eigen3::Eigen ) -target_link_libraries(mesh_msgs_transform - ${catkin_LIBRARIES} +target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_MSGS_TRANSFORM_BUILDING_LIBRARY")# + +ament_target_dependencies(${PROJECT_NAME} PUBLIC + rclcpp + mesh_msgs + tf2 + tf2_ros + geometry_msgs ) -add_dependencies(mesh_msgs_transform ${catkin_EXPORTED_TARGETS}) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -install(TARGETS mesh_msgs_transform - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +ament_export_dependencies( + rclcpp + mesh_msgs + tf2 + tf2_ros + geometry_msgs ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/mesh_msgs_transform/COLCON_IGNORE b/mesh_msgs_transform/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h b/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h index 8f6c66f..5c4a507 100644 --- a/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h +++ b/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h @@ -46,17 +46,20 @@ #ifndef MESH_MSGS_TRANSFORM__TRANSFORMS_H_ #define MESH_MSGS_TRANSFORM__TRANSFORMS_H_ -#include -#include +#include +#include -namespace mesh_msgs_transform{ - bool transformGeometryMeshNoTime( - const std::string& target_frame, - const mesh_msgs::MeshGeometryStamped& mesh_in, - const std::string& fixed_frame, - mesh_msgs::MeshGeometryStamped& mesh_out, - const tf::TransformListener& tf_listener - ); -} +namespace mesh_msgs_transform +{ -#endif /* transforms.h */ +bool transformGeometryMeshNoTime( + const std::string& target_frame, + const mesh_msgs::msg::MeshGeometryStamped& mesh_in, + const std::string& fixed_frame, + mesh_msgs::msg::MeshGeometryStamped& mesh_out, + const tf2_ros::Buffer& tf_buffer +); + +} // namespace mesh_msgs_transform + +#endif // MESH_MSGS_TRANSFORM__TRANSFORMS_H_ diff --git a/mesh_msgs_transform/package.xml b/mesh_msgs_transform/package.xml index ad5ff94..d7fca0c 100644 --- a/mesh_msgs_transform/package.xml +++ b/mesh_msgs_transform/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs_transform 1.1.0 Methods to transform mesh_msgs @@ -8,16 +9,18 @@ http://wiki.ros.org/ros_mesh_tools/mesh_msgs_transform Sebastian Pütz - tf - mesh_msgs - geometry_msgs - eigen + ament_cmake + rclcpp + tf2 + tf2_ros + mesh_msgs + geometry_msgs + eigen - tf - mesh_msgs - geometry_msgs - eigen - - catkin + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/mesh_msgs_transform/src/transforms.cpp b/mesh_msgs_transform/src/transforms.cpp index b07c7db..726a5c7 100644 --- a/mesh_msgs_transform/src/transforms.cpp +++ b/mesh_msgs_transform/src/transforms.cpp @@ -42,26 +42,35 @@ * author: Sebastian Pütz */ - - #include "mesh_msgs_transform/transforms.h" -#include +#include +#include +#include namespace mesh_msgs_transform{ -inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec){ +inline void vectorTfToEigen( + const tf2::Vector3& tf_vec, + Eigen::Vector3d& eigen_vec) +{ eigen_vec(0) = tf_vec[0]; eigen_vec(1) = tf_vec[1]; eigen_vec(2) = tf_vec[2]; } -inline void pointMsgToEigen(const geometry_msgs::Point& gm_point, Eigen::Vector3d& eigen_point){ +inline void pointMsgToEigen( + const geometry_msgs::msg::Point& gm_point, + Eigen::Vector3d& eigen_point) +{ eigen_point(0) = gm_point.x; eigen_point(1) = gm_point.y; eigen_point(2) = gm_point.z; } -inline void pointEigenToMsg(const Eigen::Vector3d& eigen_point, geometry_msgs::Point& gm_point){ +inline void pointEigenToMsg( + const Eigen::Vector3d& eigen_point, + geometry_msgs::msg::Point& gm_point) +{ gm_point.x = eigen_point(0); gm_point.y = eigen_point(1); gm_point.z = eigen_point(2); @@ -69,48 +78,41 @@ inline void pointEigenToMsg(const Eigen::Vector3d& eigen_point, geometry_msgs::P bool transformGeometryMeshNoTime( const std::string& target_frame, - const mesh_msgs::MeshGeometryStamped& mesh_in, + const mesh_msgs::msg::MeshGeometryStamped& mesh_in, const std::string& fixed_frame, - mesh_msgs::MeshGeometryStamped& mesh_out, - const tf::TransformListener& tf_listener -) + mesh_msgs::msg::MeshGeometryStamped& mesh_out, + const tf2_ros::Buffer& tf_buffer) { - tf::StampedTransform transform; - try{ - tf_listener.lookupTransform ( - target_frame, - ros::Time(0), mesh_in.header.frame_id, - ros::Time(0), - fixed_frame, - transform - ); - } - catch (tf::LookupException &e){ - ROS_ERROR ("%s", e.what ()); - return false; + geometry_msgs::msg::TransformStamped transform; + try + { + transform = tf_buffer.lookupTransform( + target_frame, tf2::TimePointZero, + mesh_in.header.frame_id, tf2::TimePointZero, + fixed_frame); } - catch (tf::ExtrapolationException &e){ - ROS_ERROR ("%s", e.what ()); + catch(const tf2::TransformException& ex) + { + RCLCPP_ERROR(rclcpp::get_logger("mesh_msgs_transform"), "%s", ex.what()); return false; } - tf::Quaternion quaternion = transform.getRotation (); - Eigen::Quaterniond rotation ( - quaternion.w (), - quaternion.x (), - quaternion.y (), - quaternion.z () - ); + Eigen::Quaterniond rotation( + transform.transform.rotation.w, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z); + + Eigen::Translation3d translation( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z ); - tf::Vector3 origin = transform.getOrigin (); - Eigen::Vector3d eigen_origin; - vectorTfToEigen(origin, eigen_origin); - Eigen::Translation3d translation ( eigen_origin ); Eigen::Affine3d transformation = translation * rotation; - if (&mesh_in != &mesh_out){ + if(&mesh_in != &mesh_out) + { mesh_out.header = mesh_in.header; - mesh_out.header.stamp = ros::Time::now(); mesh_out.mesh_geometry.faces = mesh_in.mesh_geometry.faces; } @@ -136,7 +138,7 @@ bool transformGeometryMeshNoTime( } mesh_out.header.frame_id = target_frame; - mesh_out.header.stamp = ros::Time::now(); + mesh_out.header.stamp = mesh_in.header.stamp;; return true; } From 6d83bfb1df702a098505f98af6e711252ce11d29 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 16:54:40 +0100 Subject: [PATCH 38/40] added some prints. still segfault for cluster label tool --- .../ClusterLabelTool.hpp | 2 +- .../ClusterLabelVisual.hpp | 11 +++- .../src/ClusterLabelDisplay.cpp | 25 ++++++-- .../src/ClusterLabelTool.cpp | 44 +++++++++++-- .../src/ClusterLabelVisual.cpp | 56 ++++++++++++----- rviz_mesh_tools_plugins/src/MapDisplay.cpp | 17 +---- rviz_mesh_tools_plugins/src/MeshVisual.cpp | 63 +++++++++++++++---- 7 files changed, 160 insertions(+), 58 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 ba4b8e6..c734f72 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 @@ -144,7 +144,7 @@ class ClusterLabelTool : public rviz_common::Tool /** * @brief Destructor */ - ~ClusterLabelTool(); + virtual ~ClusterLabelTool(); /** * @brief RViz callback on initialize 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 810ad07..a280ef7 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 @@ -86,7 +86,9 @@ class ClusterLabelVisual * @param context The context that contains the display information. * @param labelId The label id (that has to be unique) */ - ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId); + // ClusterLabelVisual( + // rviz_common::DisplayContext* context, + // std::string labelId); /** * @brief Constructor @@ -95,12 +97,15 @@ class ClusterLabelVisual * @param labelId The label id (that has to be unique) * @param geometry A shared pointer to the geometry to which the labels belong */ - ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId, std::shared_ptr geometry); + ClusterLabelVisual( + rviz_common::DisplayContext* context, + std::string labelId, + std::shared_ptr geometry); /** * @brief Destructor */ - ~ClusterLabelVisual(); + virtual ~ClusterLabelVisual(); /** * @brief Disabling the copy constructor diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 10027b0..2f75eb2 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -148,8 +148,10 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector has_data = true; // Draw visuals - if (isEnabled()) + if(isEnabled()) + { updateMap(); + } setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); } @@ -200,33 +202,46 @@ void ClusterLabelDisplay::updateMap() { RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Update"); - if (!has_data) + if(!has_data) { RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: No data available! Can't show map"); return; } - // Reset the visual of the label tool so that it can be deleted - m_tool->resetVisual(); + if(m_tool) + { + std::cout << "Reset Label Tool visual" << std::endl; + // Reset the visual of the label tool so that it can be deleted + m_tool->resetVisual(); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Tool not initialized!"); + } + std::cout << "createVisualsFromClusterList" << std::endl; // Now create the visuals for the loaded clusters createVisualsFromClusterList(); + std::cout << "fillPropertyOptions" << std::endl; // Fill options for dropdown menu containing the cluster names fillPropertyOptions(); + std::cout << "updatePhantomVisual" << std::endl; // Create a phantom visual if it is enabled updatePhantomVisual(); + std::cout << "notifyLabelTool" << std::endl; // Notify label tool for changes. The label tool should now destroy its visual and get a new one from this obj notifyLabelTool(); + std::cout << "updateColors" << std::endl; // Apply the default colors to the visuals updateColors(); + std::cout << "tool set display" << std::endl; // Update the tool's assigned display (to this display) m_tool->setDisplay(this); + std::cout << "All good" << std::endl; // All good setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); } @@ -353,7 +368,7 @@ void ClusterLabelDisplay::initializeLabelTool() void ClusterLabelDisplay::notifyLabelTool() { - m_tool->setVisual(m_visuals[m_activeVisualId]); + m_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 eef383f..e30117e 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -468,6 +468,7 @@ void ClusterLabelTool::selectMultipleFaces( // single face selectSingleFace(event, selectMode); } else { + std::cout << "PICK!" << std::endl; rviz_common::interaction::M_Picked pick_results; auto manager = context_->getSelectionManager(); @@ -494,6 +495,8 @@ void ClusterLabelTool::selectMultipleFaces( } // TODO: continue implementing this + } else { + std::cout << "NOTHING to pick :(" << std::endl; } } @@ -582,8 +585,39 @@ void ClusterLabelTool::selectSingleFace( // Ogre::Ray ray = event.panel->getViewController()->getCamera()->getCameraToViewportRay( // (float)event.x / event.panel->getRenderWindow()->width(), (float)event.y / event.panel->getRenderWindow()->height() // ); - Ogre::Ray ray = getMouseEventRay(event); - selectSingleFaceParallel(ray, selectMode); + Ogre::Ray mouse_ray = getMouseEventRay(event); + + Intersection intersection; + if(selectFace(context_, mouse_ray, intersection)) + { + std::cout << "selectSingleFace- HIT!" << std::endl; + + if (m_displayInitialized && m_visual) + { + + std::vector tmpFaceList; + if(m_faceSelectedArray.size() <= intersection.face_id) + { + // TODO: what is this? wtf + m_faceSelectedArray.resize(intersection.face_id + 1); + } + m_faceSelectedArray[intersection.face_id] = 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", intersection.face_id); + } + + } else { + std::cout << "selectSingleFace - No hit :(" << std::endl; + } } void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) @@ -642,7 +676,6 @@ void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) } m_visual->setFacesInCluster(tmpFaceList); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "selectSingleFaceParallel() found face with id %d", closestFaceId); } } @@ -858,7 +891,7 @@ std::vector ClusterLabelTool::getSelectedFaces() void ClusterLabelTool::resetFaces() { m_faceSelectedArray.clear(); - if (m_visual) + if(m_visual) { m_visual->setFacesInCluster(std::vector()); } @@ -866,7 +899,10 @@ void ClusterLabelTool::resetFaces() void ClusterLabelTool::resetVisual() { + // TODO: Segfault here + // m_visual.reset(); m_visual.reset(); + // std::cout << "hfe" << std::endl; } } // 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 a96fbee..58decef 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -60,15 +60,23 @@ namespace rviz_mesh_tools_plugins { -ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId) - : m_displayContext(context), m_labelId(labelId) +// ClusterLabelVisual::ClusterLabelVisual( +// rviz_common::DisplayContext* context, +// std::string labelId) +// :m_displayContext(context) +// ,m_labelId(labelId) +// { +// } + +ClusterLabelVisual::ClusterLabelVisual( + rviz_common::DisplayContext* context, + std::string labelId, + std::shared_ptr geometry) +:m_displayContext(context) +,m_labelId(labelId) +,m_geometry(geometry) { -} -ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std::string labelId, - std::shared_ptr geometry) - : m_displayContext(context), m_labelId(labelId), m_geometry(geometry) -{ // Get or create scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); Ogre::SceneNode* rootNode = sceneManager->getRootSceneNode(); @@ -88,7 +96,7 @@ ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std // Retrieve or create the mesh and attach it to the scene node m_mesh = Ogre::MeshManager::getSingleton().getByName("ClusterLabelMesh", "General"); - if (m_mesh.isNull() && geometry) + if(m_mesh.isNull() && geometry) { m_mesh = Ogre::MeshManager::getSingleton().createManual("ClusterLabelMesh", "General"); @@ -134,6 +142,10 @@ ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std // Create an entity of the mesh and add it to the scene Ogre::Entity* entity = sceneManager->createEntity("ClusterLabelEntity", "ClusterLabelMesh", "General"); + if(!entity) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "nullptr return: sceneManager->createEntity(\"ClusterLabelEntity\", \"ClusterLabelMesh\", \"General\"); "); + } entity->setMaterialName("CustomMaterial", "General"); m_sceneNode->attachObject(entity); } @@ -145,8 +157,13 @@ ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std m_subMesh->useSharedVertices = true; std::stringstream sstm; sstm << "ClusterLabel_Material_" << m_labelId; - m_material = Ogre::MaterialManager::getSingleton().create( + + m_material = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + if(m_material.isNull()) + { + m_material = Ogre::MaterialManager::getSingleton().create( sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); + } m_subMesh->setMaterialName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); m_material->getTechnique(0)->removeAllPasses(); @@ -160,6 +177,7 @@ ClusterLabelVisual::ClusterLabelVisual(rviz_common::DisplayContext* context, std ClusterLabelVisual::~ClusterLabelVisual() { + std::cout << "~ClusterLabelVisual" << std::endl; reset(); if (!m_mesh.isNull()) @@ -181,20 +199,24 @@ ClusterLabelVisual::~ClusterLabelVisual() RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); } + + std::cout << "~ClusterLabelVisual -- done" << std::endl; } void ClusterLabelVisual::reset() { RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Reset ClusterLabelVisual"); - if (!m_material.isNull()) + if(!m_material.isNull()) { - if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) - { - materialptr->unload(); - Ogre::MaterialManager::getSingleton().remove(materialptr); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << m_material->getName() << "' to unload. skipping"); - } + // if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) + // { + m_material->unload(); + Ogre::MaterialManager::getSingleton().remove(m_material); + // } else { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << m_material->getName() << "' to unload. skipping"); + // } + } else { + } } diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp index 85d45f2..7e3c9cf 100644 --- a/rviz_mesh_tools_plugins/src/MapDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -98,20 +98,11 @@ std::shared_ptr MapDisplay::getGeometry() rviz_common::Display* MapDisplay::createDisplay(const QString& class_id) { - // rviz_common::DisplayFactory* factory = context_->getDisplayFactory(); - // QString error; - - // rviz_common::Display* disp = factory->make(class_id, &error); - - // rviz_common::Display* disp = context_->getDisplayFactory()->make(class_id, &error); - rviz_common::Display* disp = context_->getRootDisplayGroup()->createDisplay(class_id); if (!disp) { - - std::cerr << "ERROR: IM SEARCHING FOR rviz_common::FailedDisplay" << std::endl; - // return new rviz_common::FailedDisplay(class_id, error); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "IM SEARCHING FOR rviz_common::FailedDisplay"); } return disp; } @@ -189,11 +180,7 @@ void MapDisplay::disableMeshDisplay() void MapDisplay::onInitialize() { std::string name = this->getName().toStdString(); - RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); - - // enableClusterLabelDisplay(); - // enableMeshDisplay(); } void MapDisplay::onEnable() @@ -244,7 +231,7 @@ void MapDisplay::load(const rviz_common::Config& config) { config2.mapSetValue(m_mapFilePath->getName(), QString::fromStdString(mesh_file) ); } else { - std::cout << name << ": COULDN'T FOUND MESH TO LOAD" << std::endl; + std::cout << name << ": COULDN'T FIND MESH TO LOAD" << std::endl; } } diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp index e0c08e9..954c5a1 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -197,12 +197,22 @@ void MeshVisual::reset() std::stringstream sstm; sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "GeneralMaterial_"; - if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str())) + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) { materialptr->unload(); Ogre::MaterialManager::getSingleton().remove(materialptr); } else { - std::cout << "Could not find material '" << sstm.str() << "' to onload. skipping" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping."); + + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + } + } sstm.str(""); @@ -212,12 +222,20 @@ void MeshVisual::reset() { sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "Material_" << 1; - if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str())) + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) { materialptr->unload(); Ogre::MaterialManager::getSingleton().remove(materialptr); } else { - std::cout << "Could not find material '" << sstm.str() << "' to unload. skipping" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping"); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + } } sstm.str(""); @@ -225,12 +243,20 @@ void MeshVisual::reset() } sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "NormalMaterial"; - if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str())) + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) { materialptr->unload(); Ogre::MaterialManager::getSingleton().remove(materialptr); } else { - std::cout << "Could not find material '" << sstm.str() << "' to unload. skipping" << std::endl; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping"); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + } } sstm.str(""); @@ -275,6 +301,15 @@ void MeshVisual::reset() m_texture_coords_enabled = false; m_textures_enabled = false; m_vertex_costs_enabled = false; + + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "MeshVisual reset done. Left materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + } } void MeshVisual::showWireframe(Ogre::Pass* pass, Ogre::ColourValue wireframeColor, float wireframeAlpha) @@ -371,11 +406,12 @@ void MeshVisual::updateMaterial(bool showFaces, Ogre::ColourValue facesColor, fl } } -void MeshVisual::updateMaterial(bool showWireframe, Ogre::ColourValue wireframeColor, float wireframeAlpha, - bool showFaces, Ogre::ColourValue facesColor, float facesAlpha, - bool useVertexColors, bool showVertexCosts, bool showTextures, - bool showTexturedFacesOnly, bool showNormals, Ogre::ColourValue normalsColor, - float normalsAlpha, float normalsScalingFactor) +void MeshVisual::updateMaterial( + bool showWireframe, Ogre::ColourValue wireframeColor, float wireframeAlpha, + bool showFaces, Ogre::ColourValue facesColor, float facesAlpha, + bool useVertexColors, bool showVertexCosts, bool showTextures, + bool showTexturedFacesOnly, bool showNormals, Ogre::ColourValue normalsColor, + float normalsAlpha, float normalsScalingFactor) { // remove all passes if (!m_meshGeneralMaterial.isNull()) @@ -744,15 +780,16 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector void MeshVisual::enteringNormals(const Geometry& mesh, const vector& normals) { - if (!m_vertex_normals_enabled) + if(!m_vertex_normals_enabled) { return; } std::stringstream sstm; - if (m_normalMaterial.isNull()) + if(m_normalMaterial.isNull()) { sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "NormalMaterial"; + m_normalMaterial = Ogre::MaterialManager::getSingleton().create( sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); m_normalMaterial->getTechnique(0)->removeAllPasses(); From 791b8c74d71055317fa584e05c986d635ad77798 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 17:14:20 +0100 Subject: [PATCH 39/40] label tool for single clicks is working again --- .../src/ClusterLabelTool.cpp | 6 +----- .../src/ClusterLabelVisual.cpp | 3 +-- .../src/InteractionHelper.cpp | 18 ++++++++++++------ 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index e30117e..b1375c2 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -580,11 +580,7 @@ void ClusterLabelTool::selectSingleFace( 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() - // ); + Ogre::Ray mouse_ray = getMouseEventRay(event); Intersection intersection; diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index 58decef..c107c74 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -228,7 +228,7 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) { m_faces = faces; - if (!m_geometry) + if(!m_geometry) { RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); return; @@ -251,7 +251,6 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) m_material->getTechnique(0)->createPass(); m_material->setDiffuse(m_color); m_material->setSelfIllumination(m_color); - initMaterial(); } diff --git a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp index 05e0172..a3c151f 100644 --- a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp +++ b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp @@ -100,19 +100,25 @@ bool selectFace( getRawManualObjectData(mesh, i, vertex_count, vertices, index_count, indices); if (index_count != 0) { - for (size_t j = 0; j < index_count; j += 3) + for (size_t face_id = 0; face_id < index_count / 3; face_id++) { - std::pair goal = Ogre::Math::intersects(ray, vertices[indices[j]], vertices[indices[j + 1]], - vertices[indices[j + 2]], true, true); + // face: indices[j], + const Ogre::Vector3 vertex; + + std::pair goal = Ogre::Math::intersects(ray, + vertices[indices[face_id * 3 + 0]], + vertices[indices[face_id * 3 + 1]], + vertices[indices[face_id * 3 + 2]], true, true); if (goal.first) { if ((dist < 0.0f) || (goal.second < dist)) { dist = goal.second; - a = vertices[indices[j]]; - b = vertices[indices[j + 1]]; - c = vertices[indices[j + 2]]; + a = vertices[indices[face_id * 3 + 0]]; + b = vertices[indices[face_id * 3 + 1]]; + c = vertices[indices[face_id * 3 + 2]]; + intersection.face_id = face_id; } } } From c6fd1897eed1181c59476271defce86c7b53d11d Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 14 Nov 2023 22:59:49 +0100 Subject: [PATCH 40/40] used rclcpp prints --- rviz_mesh_tools_plugins/CMakeLists.txt | 2 +- .../ClusterLabelTool.hpp | 8 +++---- .../src/ClusterLabelDisplay.cpp | 22 ++++--------------- .../src/ClusterLabelTool.cpp | 22 ++++++++----------- .../src/ClusterLabelVisual.cpp | 2 -- .../src/InteractionHelper.cpp | 1 - rviz_mesh_tools_plugins/src/MeshVisual.cpp | 17 +++++++------- 7 files changed, 26 insertions(+), 48 deletions(-) diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt index 17d3a43..b5a5195 100644 --- a/rviz_mesh_tools_plugins/CMakeLists.txt +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -33,7 +33,7 @@ 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) +# find_package(OpenCL 2 REQUIRED) # include_directories(${ASSIMP_INCLUDE_DIRS}) 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 c734f72..9741b00 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 @@ -54,10 +54,10 @@ // 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 +// #define CL_HPP_TARGET_OPENCL_VERSION 120 +// #define CL_HPP_MINIMUM_OPENCL_VERSION 110 +// #define CL_HPP_ENABLE_EXCEPTIONS +// #include #include #include diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 2f75eb2..e838685 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -161,10 +161,8 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector void ClusterLabelDisplay::onInitialize() { - std::cout << "ClusterLabelDisplay::onInitialize()" << std::endl; // Look for an existing label tool or create a new one initializeLabelTool(); - std::cout << "-DONE" << std::endl; } void ClusterLabelDisplay::onEnable() @@ -210,38 +208,30 @@ void ClusterLabelDisplay::updateMap() if(m_tool) { - std::cout << "Reset Label Tool visual" << std::endl; // Reset the visual of the label tool so that it can be deleted m_tool->resetVisual(); } else { RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Tool not initialized!"); } - std::cout << "createVisualsFromClusterList" << std::endl; // Now create the visuals for the loaded clusters createVisualsFromClusterList(); - std::cout << "fillPropertyOptions" << std::endl; // Fill options for dropdown menu containing the cluster names fillPropertyOptions(); - std::cout << "updatePhantomVisual" << std::endl; // Create a phantom visual if it is enabled updatePhantomVisual(); - std::cout << "notifyLabelTool" << std::endl; // Notify label tool for changes. The label tool should now destroy its visual and get a new one from this obj notifyLabelTool(); - std::cout << "updateColors" << std::endl; // Apply the default colors to the visuals updateColors(); - std::cout << "tool set display" << std::endl; // Update the tool's assigned display (to this display) m_tool->setDisplay(this); - std::cout << "All good" << std::endl; // All good setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); } @@ -344,7 +334,6 @@ void ClusterLabelDisplay::initializeLabelTool() { if (toolClasses[i].contains("ClusterLabel")) { - std::cout << "Convert " << toolClasses[i].toStdString() << std::endl; m_tool = static_cast(toolManager->getTool(i)); foundTool = true; break; @@ -353,15 +342,12 @@ void ClusterLabelDisplay::initializeLabelTool() if (!foundTool) { - std::cout << "Create new tool" << std::endl; - auto bla = context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel"); - std::cout << "bla" << std::endl; - - if(m_tool = dynamic_cast(bla); m_tool != nullptr) + auto tool_tmp = context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel"); + if(m_tool = dynamic_cast(tool_tmp); m_tool != nullptr) { - std::cout << "SUCCESS" << std::endl; + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Created ClusterLabelTool"); } else { - std::cout << "Could not create ClusterLabelTool :(" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not create ClusterLabelTool"); } } } diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp index b1375c2..7a7fe98 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -82,7 +82,7 @@ using std::stringstream; namespace rviz_mesh_tools_plugins { -#define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" +// #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" @@ -138,7 +138,6 @@ void ClusterLabelTool::onInitialize() m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); - // std::cout << "ClusterLabelTool INIT kdwioh" << std::endl; // // try-catch block to check for OpenCL errors // try @@ -468,7 +467,7 @@ void ClusterLabelTool::selectMultipleFaces( // single face selectSingleFace(event, selectMode); } else { - std::cout << "PICK!" << std::endl; + // std::cout << "PICK!" << std::endl; rviz_common::interaction::M_Picked pick_results; auto manager = context_->getSelectionManager(); @@ -482,21 +481,21 @@ void ClusterLabelTool::selectMultipleFaces( if(!pick_results.empty()) { // FOUND SOMETHING! - std::cout << "FOUND SOMETHING! " << pick_results.size() << std::endl; + // std::cout << "FOUND SOMETHING! " << pick_results.size() << std::endl; 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; + // std::cout << key << std::endl; + // std::cout << "Extra handles: " << value.extra_handles.size() << std::endl; } // TODO: continue implementing this } else { - std::cout << "NOTHING to pick :(" << std::endl; + // std::cout << "NOTHING to pick :(" << std::endl; } } @@ -586,7 +585,7 @@ void ClusterLabelTool::selectSingleFace( Intersection intersection; if(selectFace(context_, mouse_ray, intersection)) { - std::cout << "selectSingleFace- HIT!" << std::endl; + // std::cout << "selectSingleFace- HIT!" << std::endl; if (m_displayInitialized && m_visual) { @@ -612,7 +611,7 @@ void ClusterLabelTool::selectSingleFace( } } else { - std::cout << "selectSingleFace - No hit :(" << std::endl; + // std::cout << "selectSingleFace - No hit :(" << std::endl; } } @@ -840,7 +839,6 @@ int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) } else if (event.leftUp() && m_multipleSelect) { - std::cout << "BLA" << std::endl; m_multipleSelect = false; selectMultipleFaces(event, true); } @@ -895,10 +893,8 @@ void ClusterLabelTool::resetFaces() void ClusterLabelTool::resetVisual() { - // TODO: Segfault here - // m_visual.reset(); + // TODO: Segfault here, when using RViz config m_visual.reset(); - // std::cout << "hfe" << std::endl; } } // 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 c107c74..2b5c17f 100644 --- a/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -177,7 +177,6 @@ ClusterLabelVisual::ClusterLabelVisual( ClusterLabelVisual::~ClusterLabelVisual() { - std::cout << "~ClusterLabelVisual" << std::endl; reset(); if (!m_mesh.isNull()) @@ -200,7 +199,6 @@ ClusterLabelVisual::~ClusterLabelVisual() m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); } - std::cout << "~ClusterLabelVisual -- done" << std::endl; } void ClusterLabelVisual::reset() diff --git a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp index a3c151f..9afbdaf 100644 --- a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp +++ b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp @@ -169,7 +169,6 @@ bool selectFace( return false; } - bool selectFace( const rviz_common::DisplayContext* ctx, const Ogre::Ray& ray, diff --git a/rviz_mesh_tools_plugins/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp index 954c5a1..0642858 100644 --- a/rviz_mesh_tools_plugins/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -204,13 +204,12 @@ void MeshVisual::reset() } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping."); - - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); while(mit.hasMoreElements()) { Ogre::ResourcePtr material = mit.getNext(); - std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); } } @@ -229,12 +228,12 @@ void MeshVisual::reset() } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping"); - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); while(mit.hasMoreElements()) { Ogre::ResourcePtr material = mit.getNext(); - std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); } } @@ -250,12 +249,12 @@ void MeshVisual::reset() } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping"); - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); while(mit.hasMoreElements()) { Ogre::ResourcePtr material = mit.getNext(); - std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); } } @@ -303,12 +302,12 @@ void MeshVisual::reset() m_vertex_costs_enabled = false; - RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "MeshVisual reset done. Left materials are:"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "MeshVisual reset done. Left materials are:"); auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); while(mit.hasMoreElements()) { Ogre::ResourcePtr material = mit.getNext(); - std::cout << "- " << material->getName() << ", group: " << material->getGroup() << std::endl; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); } }