├── .github └── workflows │ ├── humble.yaml │ └── jazzy.yaml ├── LICENSE ├── README.md ├── label_manager ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── label_manager │ │ └── manager.h ├── launch │ └── label_manager.launch ├── package.xml ├── src │ ├── manager.cpp │ └── manager_node.cpp └── srv │ ├── DeleteLabel.srv │ ├── GetLabelGroups.srv │ └── GetLabeledClusterGroup.srv ├── mesh_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── msg │ ├── MeshFaceCluster.msg │ ├── MeshFaceClusterStamped.msg │ ├── MeshGeometry.msg │ ├── MeshGeometryStamped.msg │ ├── MeshMaterial.msg │ ├── MeshMaterials.msg │ ├── MeshMaterialsStamped.msg │ ├── MeshTexture.msg │ ├── MeshTriangleIndices.msg │ ├── MeshVertexColors.msg │ ├── MeshVertexColorsStamped.msg │ ├── MeshVertexCosts.msg │ ├── MeshVertexCostsStamped.msg │ ├── MeshVertexTexCoords.msg │ ├── VectorField.msg │ └── VectorFieldStamped.msg ├── package.xml └── srv │ ├── GetGeometry.srv │ ├── GetLabeledClusters.srv │ ├── GetMaterials.srv │ ├── GetTexture.srv │ ├── GetUUIDs.srv │ ├── GetVertexColors.srv │ ├── GetVertexCostLayers.srv │ └── GetVertexCosts.srv ├── mesh_msgs_conversions ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mesh_msgs_conversions │ │ └── conversions.h ├── package.xml └── src │ └── conversions.cpp ├── mesh_msgs_transform ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── mesh_msgs_transform │ │ └── transforms.h ├── package.xml └── src │ └── transforms.cpp ├── mesh_tools ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── rviz_mesh_tools_plugins ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── icons │ └── classes │ │ ├── ClusterLabel.png │ │ ├── Map3D.png │ │ ├── Mesh.png │ │ ├── MeshGoal.png │ │ └── MeshPoseGuess.png ├── include │ └── rviz_mesh_tools_plugins │ │ ├── ClusterLabelDisplay.hpp │ │ ├── ClusterLabelPanel.hpp │ │ ├── ClusterLabelTool.hpp │ │ ├── ClusterLabelVisual.hpp │ │ ├── InteractionHelper.hpp │ │ ├── MapDisplay.hpp │ │ ├── MeshDisplay.hpp │ │ ├── MeshGoalTool.hpp │ │ ├── MeshPoseGuessTool.hpp │ │ ├── MeshPoseTool.hpp │ │ ├── MeshVisual.hpp │ │ ├── RVizMessageFilter.hpp │ │ ├── RvizFileProperty.hpp │ │ ├── Types.hpp │ │ └── kernels │ │ └── cast_rays.cl ├── package.xml ├── plugins_description.xml ├── rviz_mesh_tools_plugins-extras.cmake └── src │ ├── ClusterLabelDisplay.cpp │ ├── ClusterLabelPanel.cpp │ ├── ClusterLabelTool.cpp │ ├── ClusterLabelVisual.cpp │ ├── InteractionHelper.cpp │ ├── MapDisplay.cpp │ ├── MeshDisplay.cpp │ ├── MeshGoalTool.cpp │ ├── MeshPoseGuessTool.cpp │ ├── MeshPoseTool.cpp │ ├── MeshVisual.cpp │ └── RvizFileProperty.cpp └── source_dependencies.yaml /.github/workflows/humble.yaml: -------------------------------------------------------------------------------- 1 | name: Humble CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - 'main' 7 | pull_request: 8 | workflow_dispatch: 9 | branches: 10 | - '*' 11 | 12 | jobs: 13 | humble_build_and_test: 14 | uses: naturerobots/github_automation_public/.github/workflows/ros_ci.yaml@main 15 | secrets: inherit 16 | with: 17 | ros_distro: humble 18 | -------------------------------------------------------------------------------- /.github/workflows/jazzy.yaml: -------------------------------------------------------------------------------- 1 | name: Jazzy CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - 'main' 7 | pull_request: 8 | workflow_dispatch: 9 | branches: 10 | - '*' 11 | 12 | jobs: 13 | jazzy_build_and_test: 14 | uses: naturerobots/github_automation_public/.github/workflows/ros_ci.yaml@main 15 | secrets: inherit 16 | with: 17 | ros_distro: jazzy 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Osnabrück University, Sebastian Pütz 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Jazzy CI](https://github.com/naturerobots/mesh_tools/actions/workflows/jazzy.yaml/badge.svg)](https://github.com/naturerobots/mesh_tools/actions/workflows/jazzy.yaml) 2 | [![Humble CI](https://github.com/naturerobots/mesh_tools/actions/workflows/humble.yaml/badge.svg)](https://github.com/naturerobots/mesh_tools/actions/workflows/humble.yaml) 3 | 4 | This is the active ROS2 branch of this repository. 5 | If your are looking for the old ROS1 version, checkout the [noetic branch](https://github.com/naturerobots/mesh_tools/tree/noetic). 6 | 7 | # Mesh Tools 8 | 9 | We introduce a set of tools to make 3D environment mesh representations 10 | accessible and manageable in ROS. We provide RViz tools to visualize and 11 | annotate huge meshes in combination with generated textures and different 12 | cost layers, which are based on the geometric analyses of the environment, 13 | or which represent different sensor readings, e.g. RGB image or even 14 | hyper-spectral image textures. 15 | 16 | ## Introduction 17 | 18 | Over the last years, the Knowledge Based Systems Group has developed 19 | an extensive set of tools to automatically generate triangle meshes 20 | from 3D laser scans. These tools are implemented in the 21 | freely available Las Vegas Surface Reconstruction Toolkit 22 | (LVR) and have been successfully applied in different 23 | robotic contexts. 24 | 25 | With terrestrial and mobile laser scanning it is possible to create 3D 26 | point clouds of large environments in short time. However, the amount 27 | of collected data is too large to be processed on a mobile robot. To 28 | make this data available on mobile robots, we suggest to compute 3D 29 | triangle meshes from point cloud data as corresponding map 30 | representations. Such maps have several advantages 31 | over raw point cloud data and voxel representations. They overcome the 32 | discretization of voxel maps and deliver topologically connected 33 | surface representations, which ease segmentation, annotation and enable intuitive visualization. 34 | 35 | However, to make the maps accessible in ROS, corresponding message 36 | definitions and tools for storing, handling and editing such maps are 37 | required. 38 | 39 | ### Citation 40 | 41 | Please reference the following papers when using mesh_tools in your scientific work. 42 | 43 | ```latex 44 | @inproceedings{putz2019ecmr, 45 | title={{Tools for visualizing, annotating and storing triangle meshes in ROS and RViz}}, 46 | author={P{\"u}tz, Sebastian and Wiemann, Thomas and Hertzberg, Joachim}, 47 | booktitle={2019 European Conference on Mobile Robots (ECMR)}, 48 | pages={1--6}, 49 | year={2019}, 50 | organization={IEEE} 51 | } 52 | ``` 53 | 54 | ```latex 55 | @article{putz2021ras, 56 | title = {{The Mesh Tools Package – Introducing Annotated 3D Triangle Maps in ROS}}, 57 | journal = {Robotics and Autonomous Systems}, 58 | volume = {138}, 59 | pages = {103688}, 60 | year = {2021}, 61 | doi = {https://doi.org/10.1016/j.robot.2020.103688}, 62 | author = {P{\"u}tz, Sebastian and Wiemann, Thomas and Hertzberg, Joachim}, 63 | } 64 | ``` 65 | 66 | ## Message Definitions 67 | 68 | We divided the mesh structure in its geometry, textures and cost layers. These different components are then associated using a UUID. This structure enables passing the geometry at first to RViz and other nodes. Theses nodes may analyze the geometry, e.g the roughness or height difference and compute corresponding mesh cost layers which are published as individual message with a corresponding mesh UUID. Thereby other nodes can associate these layers with a certain mesh object. In RViz a certain layer can then be selected by the user and visualized coloring the corresponding costs or displaying the textures. In the following gives an overview on most important mesh messages: 69 | 70 | + **MeshGeometry(Stamped)**: Geometric structure of a triangle mesh using a buffer vertices, normals and faces 71 | + **MeshVertexColors(Stamped)**: Vertex color information of a corresponding mesh 72 | + **MeshVertexCosts(Stamped)**: Vertex cost information of a corresponding mesh coloring the mesh using a rainbow color scheme. 73 | + **ClusterLabel**: Grouping a set of triangles / faces of a corresponding mesh using the face indices, optional cluster label 74 | + **Texture**: Texture using an Image and an ID 75 | + **Material**: Color and optional texture ID 76 | + **VertexTexCoords**: Texture coordinate 77 | + **MeshMaterials(Stamped)**: Combining the materials with texture coordinates and clusters of a corresponding mesh 78 | + **Feature**: A feature at a specific location and its feature descriptor 79 | + **MeshFeatures**: List of features for a corresponding mesh 80 | 81 | ## RViz Plugins 82 | 83 | We developed several RViz plugins to display 3D meshes together 84 | with different information layers (RGB textures, semantic labels, 85 | trafficability, etc.). It is possible to render the received meshes in 86 | different modes (lighting, materials and wireframe). The supported 87 | modes are selectable based on availability in the tree view. For path 88 | planning, we also implemented an interactive tool to set navigation 89 | goal poses on the mesh surface. To generate semantic maps, we implemented an 90 | interactive tool that can be used to select connected clusters of 91 | triangles and assign semantic labels to them. Some screenshots and videos demonstrate these plugins (see Supplementary Material). 92 | 93 | ### RViz Mesh Plugin 94 | The *TexturedMesh* displays a mesh with optional a fixed color, textures, vertex colors or vertex costs. Additionally, it can display the wire-frame as well as the vertex normals of the mesh. The *MeshGoal Tool* provides the possibility to select a *geometry_msgs/PoseStamped* on the surface of the mesh. 95 | 96 | ### RViz Map Plugin 97 | The *Mesh Display* displays labeled faces. The *ClusterLabel Display* displays labeled clusters. The *ClusterLabel Tool* enables the labeling of certain faces using different selection and de-selection methods as shown in a video (see Supplementary Material. The *ClusterLabel Panel* allows to manage your clusters and label names as well as the corresponding colors. 98 | 99 | # Videos 100 | 101 | ## Mesh Cost Layer 102 | This Video shows different cost layers which are used for path planning in outdoor terrain. It shows one partly reconstructed scan in the botanical garden of Osnabrück 103 | 104 | [![Mesh Cost Layer](http://img.youtube.com/vi/Ac1YLn88QGk/0.jpg)](http://www.youtube.com/watch?v=Ac1YLn88QGk) 105 | 106 | ## Mesh Textures 107 | This Video shows a textured dataset of the botanical garden of Osnabrück including footpaths 108 | 109 | [![Mesh Textures](http://img.youtube.com/vi/CF-WdXwx_zo/0.jpg)](http://www.youtube.com/watch?v=CF-WdXwx_zo) 110 | 111 | ## Label Tool 112 | This video shows the labeling tool, which is used to cluster faces to an object or a union, (in the example a door) using a mesh inside a building 113 | 114 | [![Label Tool](http://img.youtube.com/vi/3IV2yo0D_CU/0.jpg)](http://www.youtube.com/watch?v=3IV2yo0D_CU) 115 | -------------------------------------------------------------------------------- /label_manager/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package label_manager 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.1.0 (2021-10-27) 10 | ------------------ 11 | * resolved catkin lint problems 12 | 13 | 1.0.1 (2020-11-11) 14 | ------------------ 15 | 16 | 1.0.0 (2020-04-26) 17 | ------------------ 18 | * release version 1.0.0 19 | 20 | -------------------------------------------------------------------------------- /label_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(label_manager) 3 | 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | 16 | 17 | find_package(rclcpp REQUIRED) 18 | find_package(rclcpp_action REQUIRED) 19 | find_package(rclcpp_components REQUIRED) 20 | find_package(rosidl_default_generators REQUIRED) 21 | find_package(mesh_msgs REQUIRED) 22 | find_package(sensor_msgs REQUIRED) 23 | find_package(std_msgs REQUIRED) 24 | 25 | find_package(Boost COMPONENTS 26 | system 27 | filesystem 28 | ) 29 | 30 | 31 | rosidl_generate_interfaces(${PROJECT_NAME} 32 | "srv/DeleteLabel.srv" 33 | "srv/GetLabelGroups.srv" 34 | "srv/GetLabeledClusterGroup.srv" 35 | DEPENDENCIES 36 | mesh_msgs 37 | std_msgs 38 | ADD_LINTER_TESTS 39 | ) 40 | 41 | 42 | include_directories( 43 | include 44 | ) 45 | 46 | add_executable(${PROJECT_NAME}_node 47 | src/manager.cpp 48 | src/manager_node.cpp) 49 | 50 | add_dependencies(${PROJECT_NAME}_node 51 | ${PROJECT_NAME} 52 | ) 53 | 54 | target_link_libraries(${PROJECT_NAME}_node 55 | Boost::system 56 | Boost::filesystem 57 | ) 58 | 59 | ament_target_dependencies(${PROJECT_NAME}_node 60 | rclcpp 61 | rclcpp_action 62 | rclcpp_components 63 | mesh_msgs 64 | sensor_msgs 65 | std_msgs 66 | ) 67 | 68 | install(TARGETS ${PROJECT_NAME}_node 69 | ARCHIVE DESTINATION lib/${PROJECT_NAME} 70 | LIBRARY DESTINATION lib/${PROJECT_NAME} 71 | RUNTIME DESTINATION bin/${PROJECT_NAME} 72 | ) 73 | 74 | rosidl_get_typesupport_target(cpp_typesupport_target 75 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 76 | target_link_libraries(${PROJECT_NAME}_node 77 | "${cpp_typesupport_target}") 78 | 79 | 80 | ament_export_dependencies(rosidl_default_runtime) 81 | ament_package() 82 | 83 | -------------------------------------------------------------------------------- /label_manager/README.md: -------------------------------------------------------------------------------- 1 | # label_manager 2 | 3 | This package contains a ROS node for serving and persisting label information 4 | -------------------------------------------------------------------------------- /label_manager/include/label_manager/manager.h: -------------------------------------------------------------------------------- 1 | #ifndef LABEL_MANAGER_H_ 2 | #define LABEL_MANAGER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" 11 | #include "mesh_msgs/srv/get_labeled_clusters.hpp" 12 | #include "label_manager/srv/get_label_groups.hpp" 13 | #include "label_manager/srv/get_labeled_cluster_group.hpp" 14 | #include "label_manager/srv/delete_label.hpp" 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | 18 | 19 | namespace label_manager 20 | { 21 | 22 | class LabelManager : public rclcpp::Node 23 | { 24 | public: 25 | LabelManager(std::string handle_str = ""); 26 | 27 | private: 28 | // Subscriber 29 | rclcpp::Subscription::SharedPtr 30 | clusterLabelSub; 31 | 32 | // Publisher 33 | rclcpp::Publisher::SharedPtr 34 | newClusterLabelPub; 35 | 36 | // Service (Servers) 37 | rclcpp::Service::SharedPtr 38 | srv_get_labeled_clusters; 39 | rclcpp::Service::SharedPtr 40 | srv_get_label_groups; 41 | rclcpp::Service::SharedPtr 42 | srv_get_labeled_cluster_group; 43 | rclcpp::Service::SharedPtr 44 | srv_delete_label; 45 | 46 | std::string folderPath; 47 | 48 | void clusterLabelCallback(const mesh_msgs::msg::MeshFaceClusterStamped& msg); 49 | 50 | bool service_getLabeledClusters( 51 | const std::shared_ptr req, 52 | std::shared_ptr res); 53 | bool service_getLabelGroups( 54 | const std::shared_ptr req, 55 | std::shared_ptr res); 56 | bool service_getLabeledClusterGroup( 57 | const std::shared_ptr req, 58 | std::shared_ptr res); 59 | bool service_deleteLabel( 60 | const std::shared_ptr req, 61 | std::shared_ptr res); 62 | 63 | bool writeIndicesToFile(const std::string& fileName, const std::vector& indices, const bool append); 64 | std::vector readIndicesFromFile(const std::string& fileName); 65 | std::string getFileName(const std::string& uuid, const std::string& label); 66 | }; 67 | 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /label_manager/launch/label_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /label_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | label_manager 5 | 2.0.0 6 | Serving and persisting label information 7 | Alexander Mock 8 | Matthias Holoch 9 | Sebastian Pütz 10 | 11 | http://wiki.ros.org/ros_mesh_tools/label_manager 12 | 13 | Sebastian Pütz 14 | Jan Philipp Vogtherr 15 | 16 | BSD-3 17 | 18 | ament_cmake 19 | rosidl_default_generators 20 | 21 | rclcpp 22 | rclcpp_action 23 | rclcpp_components 24 | mesh_msgs 25 | sensor_msgs 26 | std_msgs 27 | 28 | 29 | rosidl_default_runtime 30 | 31 | ament_lint_common 32 | 33 | rosidl_interface_packages 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /label_manager/src/manager.cpp: -------------------------------------------------------------------------------- 1 | #include "label_manager/manager.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "mesh_msgs/msg/mesh_face_cluster.h" 8 | 9 | using namespace boost::filesystem; 10 | 11 | using std::placeholders::_1; 12 | using std::placeholders::_2; 13 | 14 | namespace label_manager 15 | { 16 | LabelManager::LabelManager(std::string handle_str) 17 | :rclcpp::Node(handle_str) 18 | { 19 | // TODO: check if this is correct 20 | this->declare_parameter("folder_path", "/tmp/label_manager/"); 21 | folderPath = this->get_parameter("folder_path").as_string(); 22 | 23 | 24 | path p(folderPath); 25 | if(!is_directory(p) && !exists(p)) 26 | { 27 | create_directory(p); 28 | } 29 | 30 | clusterLabelSub = this->create_subscription( 31 | "cluster_label", 10, std::bind(&LabelManager::clusterLabelCallback, this, _1)); 32 | 33 | newClusterLabelPub = this->create_publisher( 34 | "new_cluster_label", 10); 35 | 36 | srv_get_labeled_clusters = this->create_service( 37 | "get_labeled_clusters", std::bind(&LabelManager::service_getLabeledClusters, this, _1, _2)); 38 | 39 | srv_get_label_groups = this->create_service( 40 | "get_label_groups", std::bind(&LabelManager::service_getLabelGroups, this, _1, _2)); 41 | 42 | srv_get_labeled_cluster_group = this->create_service( 43 | "get_labeled_cluster_group", std::bind(&LabelManager::service_getLabeledClusterGroup, this, _1, _2)); 44 | 45 | srv_delete_label = this->create_service( 46 | "delete_label", std::bind(&LabelManager::service_deleteLabel, this, _1, _2)); 47 | 48 | RCLCPP_INFO(this->get_logger(), "Started LabelManager"); 49 | } 50 | 51 | void LabelManager::clusterLabelCallback(const mesh_msgs::msg::MeshFaceClusterStamped& msg) 52 | { 53 | RCLCPP_INFO_STREAM(this->get_logger(), "Got msg for mesh: " << msg.uuid << " with label: " << msg.cluster.label); 54 | 55 | std::vector indices; 56 | std::string fileName = getFileName(msg.uuid, msg.cluster.label); 57 | 58 | // if appending (not override), first figure what new indices we have to add 59 | if (!msg.override) 60 | { 61 | std::vector readIndices = readIndicesFromFile(fileName); 62 | 63 | // if read indices is empty no file was found or could not be read 64 | if (readIndices.empty()) 65 | { 66 | indices = msg.cluster.face_indices; 67 | } 68 | else 69 | { 70 | for (size_t i = 0; i < msg.cluster.face_indices.size(); i++) 71 | { 72 | uint idx = msg.cluster.face_indices[i]; 73 | 74 | // if msg index is not already in file, add it to indices vector 75 | if (std::find(readIndices.begin(), readIndices.end(), idx) == readIndices.end()) 76 | { 77 | indices.push_back(idx); 78 | } 79 | } 80 | } 81 | } 82 | else 83 | { 84 | indices = msg.cluster.face_indices; 85 | } 86 | 87 | // publish every new labeled cluster 88 | newClusterLabelPub->publish(msg.cluster); 89 | 90 | // make sure mesh folder exists before writing 91 | path p(folderPath + "/" + msg.uuid); 92 | if (!is_directory(p) || !exists(p)) 93 | { 94 | create_directory(p); 95 | } 96 | 97 | writeIndicesToFile(fileName, indices, !msg.override); 98 | } 99 | 100 | bool LabelManager::service_getLabeledClusters( 101 | const std::shared_ptr req, 102 | std::shared_ptr res) 103 | { 104 | 105 | RCLCPP_DEBUG_STREAM(this->get_logger(), "Service call with uuid: " << req->uuid); 106 | 107 | path p (folderPath + "/" + req->uuid); 108 | directory_iterator end_itr; 109 | 110 | if (!is_directory(p) || !exists(p)) 111 | { 112 | RCLCPP_DEBUG_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); 113 | 114 | return false; 115 | } 116 | 117 | for (directory_iterator itr(p); itr != end_itr; ++itr) 118 | { 119 | // if file is no dir 120 | if (is_regular_file(itr->path())) 121 | { 122 | std::string label = itr->path().filename().string(); 123 | // remove extension from label 124 | boost::replace_all(label, itr->path().filename().extension().string(), ""); 125 | 126 | mesh_msgs::msg::MeshFaceCluster c; 127 | c.face_indices = readIndicesFromFile(itr->path().string()); 128 | c.label = label; 129 | 130 | res->clusters.push_back(c); 131 | } 132 | } 133 | 134 | return true; 135 | } 136 | 137 | bool LabelManager::service_getLabelGroups( 138 | const std::shared_ptr req, 139 | std::shared_ptr res) 140 | { 141 | path p (folderPath + "/" + req->uuid); 142 | directory_iterator end_itr; 143 | 144 | if (!is_directory(p) || !exists(p)) 145 | { 146 | RCLCPP_WARN_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); 147 | 148 | return false; 149 | } 150 | 151 | for (directory_iterator itr(p); itr != end_itr; ++itr) 152 | { 153 | // if file is no dir 154 | if (is_regular_file(itr->path())) 155 | { 156 | std::string label = itr->path().filename().string(); 157 | // remove extension from label 158 | boost::replace_all(label, itr->path().filename().extension().string(), ""); 159 | 160 | // assuming the labels will look like this: 'GROUP_SOMETHINGELSE', 161 | // remove everthing not representing the group 162 | // TODO make seperator configurable 163 | label = label.substr(0, label.find_first_of("_", 0)); 164 | 165 | // only add label group to response if not already added 166 | if (std::find(res->labels.begin(), res->labels.end(), label) == res->labels.end()) 167 | { 168 | res->labels.push_back(label); 169 | } 170 | } 171 | } 172 | 173 | 174 | return true; 175 | } 176 | 177 | bool LabelManager::service_getLabeledClusterGroup( 178 | const std::shared_ptr req, 179 | std::shared_ptr res) 180 | { 181 | path p (folderPath + "/" + req->uuid); 182 | directory_iterator end_itr; 183 | 184 | if (!is_directory(p) || !exists(p)) 185 | { 186 | RCLCPP_WARN_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); 187 | 188 | return false; 189 | } 190 | 191 | for (directory_iterator itr(p); itr != end_itr; ++itr) 192 | { 193 | // if file is no dir 194 | if (is_regular_file(itr->path()) && itr->path().filename().string().find(req->label_group) == 0) 195 | { 196 | std::string label = itr->path().filename().string(); 197 | // remove extension from label 198 | boost::replace_all(label, itr->path().filename().extension().string(), ""); 199 | 200 | mesh_msgs::msg::MeshFaceCluster c; 201 | c.face_indices = readIndicesFromFile(itr->path().string()); 202 | c.label = label; 203 | 204 | res->clusters.push_back(c); 205 | } 206 | } 207 | 208 | 209 | return true; 210 | } 211 | 212 | bool LabelManager::service_deleteLabel( 213 | const std::shared_ptr req, 214 | std::shared_ptr res) 215 | { 216 | path p(getFileName(req->uuid, req->label)); 217 | 218 | if (!is_regular_file(p) || !exists(p)) 219 | { 220 | RCLCPP_WARN_STREAM(this->get_logger(), "Could not delete label '" << req->label << "' of mesh '" << req->uuid << "'."); 221 | 222 | return false; 223 | } 224 | 225 | res->cluster.face_indices = readIndicesFromFile(p.filename().string()); 226 | res->cluster.label = req->label; 227 | 228 | return remove(p); 229 | } 230 | 231 | bool LabelManager::writeIndicesToFile( 232 | const std::string& fileName, 233 | const std::vector& indices, 234 | const bool append 235 | ) 236 | { 237 | if (indices.empty()) 238 | { 239 | RCLCPP_WARN_STREAM(this->get_logger(), "Empty indices."); 240 | 241 | return true; 242 | } 243 | 244 | std::ios_base::openmode mode = append ? (std::ios::out|std::ios::app) : std::ios::out; 245 | std::ofstream ofs(fileName.c_str(), mode); 246 | 247 | RCLCPP_DEBUG_STREAM(this->get_logger(), "Writing indices to file: " << fileName); 248 | 249 | if (ofs.is_open()) 250 | { 251 | // if in append mode add , after the old data 252 | if (append) 253 | { 254 | ofs << ","; 255 | } 256 | 257 | size_t size = indices.size(); 258 | for (size_t i = 0; i < size; i++) 259 | { 260 | ofs << indices[i]; 261 | 262 | if (i < size - 1) 263 | { 264 | ofs << ","; 265 | } 266 | } 267 | 268 | ofs.close(); 269 | RCLCPP_DEBUG_STREAM(this->get_logger(), "Successfully written indices to file."); 270 | 271 | return true; 272 | } 273 | else 274 | { 275 | RCLCPP_ERROR_STREAM(this->get_logger(), "Could not open file: " << fileName); 276 | } 277 | 278 | return false; 279 | } 280 | 281 | std::vector LabelManager::readIndicesFromFile(const std::string& fileName) 282 | { 283 | std::ifstream ifs(fileName.c_str(), std::ios::in); 284 | std::vector faceIndices; 285 | 286 | // if file dos not exists, return empty vector 287 | if (!ifs.good()) 288 | { 289 | RCLCPP_DEBUG_STREAM(this->get_logger(), "File " << fileName << " does not exists. Nothing to read..."); 290 | 291 | return faceIndices; 292 | } 293 | 294 | std::string stringNumber; 295 | while (std::getline(ifs, stringNumber, ',')) 296 | { 297 | faceIndices.push_back(atoi(stringNumber.c_str())); 298 | } 299 | 300 | return faceIndices; 301 | } 302 | 303 | std::string LabelManager::getFileName(const std::string& uuid, const std::string& label) 304 | { 305 | return folderPath + "/" +uuid + "/" + label + ".dat"; 306 | } 307 | } 308 | -------------------------------------------------------------------------------- /label_manager/src/manager_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * manager_node.cpp 3 | * 4 | */ 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "label_manager/manager.h" 8 | 9 | int main(int argc, char **argv) 10 | { 11 | rclcpp::init(argc, argv); 12 | rclcpp::spin(std::make_shared( 13 | "label_manager")); 14 | rclcpp::shutdown(); 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /label_manager/srv/DeleteLabel.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | string label 3 | --- 4 | mesh_msgs/MeshFaceCluster cluster 5 | -------------------------------------------------------------------------------- /label_manager/srv/GetLabelGroups.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | string[] labels 4 | -------------------------------------------------------------------------------- /label_manager/srv/GetLabeledClusterGroup.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | string label_group 3 | --- 4 | mesh_msgs/MeshFaceCluster[] clusters 5 | -------------------------------------------------------------------------------- /mesh_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.1.0 (2021-10-27) 10 | ------------------ 11 | * added publishing of mesh_msgs to mesh_msgs_hdf5 package 12 | * removed unused messages 13 | * correct wiki address for mesh_tools 14 | 15 | 1.0.1 (2020-11-11) 16 | ------------------ 17 | 18 | 1.0.0 (2020-04-26) 19 | ------------------ 20 | * release version 1.0.0 21 | -------------------------------------------------------------------------------- /mesh_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mesh_msgs) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | 19 | 20 | rosidl_generate_interfaces(${PROJECT_NAME} 21 | "msg/MeshFaceCluster.msg" 22 | "msg/MeshFaceClusterStamped.msg" 23 | "msg/MeshMaterial.msg" 24 | "msg/MeshGeometry.msg" 25 | "msg/MeshGeometryStamped.msg" 26 | "msg/MeshMaterials.msg" 27 | "msg/MeshMaterialsStamped.msg" 28 | "msg/MeshVertexColors.msg" 29 | "msg/MeshVertexColorsStamped.msg" 30 | "msg/MeshVertexCosts.msg" 31 | "msg/MeshVertexCostsStamped.msg" 32 | "msg/MeshTexture.msg" 33 | "msg/MeshTriangleIndices.msg" 34 | "msg/VectorField.msg" 35 | "msg/VectorFieldStamped.msg" 36 | "msg/MeshVertexTexCoords.msg" 37 | "srv/GetGeometry.srv" 38 | "srv/GetLabeledClusters.srv" 39 | "srv/GetMaterials.srv" 40 | "srv/GetTexture.srv" 41 | "srv/GetUUIDs.srv" 42 | "srv/GetVertexColors.srv" 43 | "srv/GetVertexCosts.srv" 44 | "srv/GetVertexCostLayers.srv" 45 | DEPENDENCIES 46 | std_msgs 47 | geometry_msgs 48 | sensor_msgs 49 | ) 50 | 51 | ament_export_dependencies(rosidl_default_runtime) 52 | ament_package() -------------------------------------------------------------------------------- /mesh_msgs/README.md: -------------------------------------------------------------------------------- 1 | # mesh_msgs 2 | 3 | This package contains various ROS message types to represent mesh data. 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshFaceCluster.msg: -------------------------------------------------------------------------------- 1 | #Cluster 2 | uint32[] face_indices 3 | #optional 4 | string label 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshFaceClusterStamped.msg: -------------------------------------------------------------------------------- 1 | # header 2 | std_msgs/Header header 3 | 4 | # mesh uuid 5 | string uuid 6 | 7 | # Cluster 8 | MeshFaceCluster cluster 9 | 10 | # overwrite existing labeled faces 11 | bool override 12 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshGeometry.msg: -------------------------------------------------------------------------------- 1 | # Mesh Geometry Message 2 | geometry_msgs/Point[] vertices 3 | geometry_msgs/Point[] vertex_normals 4 | mesh_msgs/MeshTriangleIndices[] faces 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshGeometryStamped.msg: -------------------------------------------------------------------------------- 1 | # Mesh Geometry Message 2 | std_msgs/Header header 3 | string uuid 4 | mesh_msgs/MeshGeometry mesh_geometry 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshMaterial.msg: -------------------------------------------------------------------------------- 1 | uint32 texture_index 2 | std_msgs/ColorRGBA color 3 | bool has_texture 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshMaterials.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | mesh_msgs/MeshFaceCluster[] clusters 3 | mesh_msgs/MeshMaterial[] materials 4 | uint32[] cluster_materials 5 | mesh_msgs/MeshVertexTexCoords[] vertex_tex_coords 6 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshMaterialsStamped.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | std_msgs/Header header 3 | string uuid 4 | mesh_msgs/MeshMaterials mesh_materials 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshTexture.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | string uuid 3 | uint32 texture_index 4 | sensor_msgs/Image image 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshTriangleIndices.msg: -------------------------------------------------------------------------------- 1 | # Definition of a triangle's vertices 2 | uint32[3] vertex_indices 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexColors.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | std_msgs/ColorRGBA[] vertex_colors 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexColorsStamped.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | std_msgs/Header header 3 | string uuid 4 | mesh_msgs/MeshVertexColors mesh_vertex_colors 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexCosts.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | float32[] costs 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexCostsStamped.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | std_msgs/Header header 3 | string uuid 4 | string type 5 | mesh_msgs/MeshVertexCosts mesh_vertex_costs 6 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexTexCoords.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Type 2 | float32 u 3 | float32 v 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/VectorField.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] positions 2 | geometry_msgs/Vector3[] vectors 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/VectorFieldStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | mesh_msgs/VectorField vector_field 3 | -------------------------------------------------------------------------------- /mesh_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mesh_msgs 5 | 2.0.0 6 | Various Message Types for Mesh Data. 7 | Alexander Mock 8 | Matthias Holoch 9 | Sebastian Pütz 10 | Sebastian Pütz 11 | Henning Deeken 12 | Jan Philipp Vogtherr 13 | Rasmus Diederichsen 14 | Christian Swan 15 | 16 | http://wiki.ros.org/mesh_tools/mesh_msgs 17 | BSD-3 18 | 19 | ament_cmake 20 | rosidl_default_generators 21 | 22 | geometry_msgs 23 | sensor_msgs 24 | std_msgs 25 | 26 | rosidl_default_runtime 27 | 28 | ament_lint_common 29 | 30 | rosidl_interface_packages 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetGeometry.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshGeometryStamped mesh_geometry_stamped 4 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetLabeledClusters.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | MeshFaceCluster[] clusters 4 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetMaterials.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshMaterialsStamped mesh_materials_stamped 4 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetTexture.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | uint32 texture_index 3 | --- 4 | mesh_msgs/MeshTexture texture 5 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetUUIDs.srv: -------------------------------------------------------------------------------- 1 | # uuids of all currently available meshes 2 | 3 | --- 4 | string[] uuids 5 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetVertexColors.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshVertexColorsStamped mesh_vertex_colors_stamped 4 | -------------------------------------------------------------------------------- /mesh_msgs/srv/GetVertexCostLayers.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | string[] layers -------------------------------------------------------------------------------- /mesh_msgs/srv/GetVertexCosts.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | string layer # costlayer (something like "roughness" or "height_diff") 3 | --- 4 | mesh_msgs/MeshVertexCostsStamped mesh_vertex_costs_stamped 5 | -------------------------------------------------------------------------------- /mesh_msgs_conversions/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_msgs_conversions 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.1.0 (2021-10-27) 10 | ------------------ 11 | * removed unused messages 12 | * resolved catkin lint problems 13 | 14 | 1.0.1 (2020-11-11) 15 | ------------------ 16 | * fix of static-name bug in LVR2Config.cmake 17 | * removed not implemented functions from header 18 | * moved lvr_ros conversions to mesh_msgs_conversions 19 | -------------------------------------------------------------------------------- /mesh_msgs_conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mesh_msgs_conversions) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # Default: Release 14 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 15 | if (NOT CMAKE_BUILD_TYPE) 16 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 17 | endif() 18 | endif() 19 | 20 | find_package(ament_cmake REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(mesh_msgs REQUIRED) 23 | find_package(sensor_msgs REQUIRED) 24 | 25 | find_package(LVR2 REQUIRED) 26 | find_package(OpenCV REQUIRED) 27 | find_package(MPI REQUIRED) 28 | find_package(PkgConfig REQUIRED) 29 | 30 | add_definitions(${LVR2_DEFINITIONS} ${OpenCV_DEFINITIONS}) 31 | 32 | # enable openmp support 33 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") 34 | include_directories( 35 | include 36 | ${LVR2_INCLUDE_DIRS} 37 | ${OpenCV_INCLUDE_DIRS} 38 | ) 39 | 40 | add_library(${PROJECT_NAME} 41 | src/conversions.cpp 42 | ) 43 | 44 | target_include_directories(${PROJECT_NAME} PUBLIC 45 | $ 46 | $) 47 | 48 | target_link_libraries(${PROJECT_NAME} PUBLIC 49 | ${LVR2_LIBRARY} 50 | ${OpenCV_LIBRARIES} 51 | rclcpp::rclcpp 52 | ${mesh_msgs_TARGETS} 53 | ${sensor_msgs_TARGETS} 54 | ) 55 | 56 | 57 | target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_MSGS_CONVERSIONS_BUILDING_DLL") 58 | 59 | install(TARGETS ${PROJECT_NAME} 60 | EXPORT export_${PROJECT_NAME} 61 | ARCHIVE DESTINATION lib 62 | LIBRARY DESTINATION lib 63 | RUNTIME DESTINATION bin 64 | ) 65 | 66 | install( 67 | DIRECTORY include/ 68 | DESTINATION include 69 | ) 70 | 71 | ament_export_include_directories("include/${PROJECT_NAME}") 72 | ament_export_libraries(${PROJECT_NAME}) 73 | 74 | # Export modern CMake targets 75 | ament_export_targets(export_${PROJECT_NAME}) 76 | 77 | ament_export_dependencies( 78 | rclcpp 79 | mesh_msgs 80 | sensor_msgs 81 | ) 82 | 83 | ament_package() -------------------------------------------------------------------------------- /mesh_msgs_conversions/README.md: -------------------------------------------------------------------------------- 1 | # mesh_msgs_conversions 2 | 3 | 4 | Conversion functions between lvr2 and mesh_msgs. 5 | 6 | 7 | ## ROS2 Port TODOs 8 | 9 | No global nodes anymore 10 | 11 | I changed 12 | 1. ros::Time::now() -> rclcpp::Time() 13 | 2. ROS_INFO -> std::cout 14 | 15 | The first point could destroy functionallity. The second one could destroy the logging. TODO: do this correctly -------------------------------------------------------------------------------- /mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 3 | * Copyright (C) 2013 University of Osnabrück 4 | * 5 | * This program is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU General Public License 7 | * as published by the Free Software Foundation; either version 2 8 | * of the License, or (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program; if not, write to the Free Software 17 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 18 | * 19 | * conversions.h 20 | * 21 | * created on: 30.04.2014 22 | * 23 | * Author: Henning Deeken , 24 | * Sebastian Pütz , 25 | * Marcel Mrozinski , 26 | * Tristan Igelbrink 27 | * 28 | */ 29 | 30 | #ifndef MESH_MSGS_CONVERSIONS_H_ 31 | #define MESH_MSGS_CONVERSIONS_H_ 32 | 33 | #include 34 | 35 | #include "rclcpp/rclcpp.hpp" 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | #include "std_msgs/msg/string.h" 52 | #include "sensor_msgs/msg/point_cloud2.hpp" 53 | #include "sensor_msgs/fill_image.hpp" 54 | 55 | #include "mesh_msgs/msg/mesh_face_cluster.hpp" 56 | #include "mesh_msgs/msg/mesh_material.hpp" 57 | #include "mesh_msgs/msg/mesh_triangle_indices.hpp" 58 | 59 | #include "mesh_msgs/msg/mesh_geometry.hpp" 60 | #include "mesh_msgs/msg/mesh_geometry_stamped.hpp" 61 | #include "mesh_msgs/msg/mesh_materials_stamped.hpp" 62 | #include "mesh_msgs/msg/mesh_vertex_colors.hpp" 63 | #include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp" 64 | #include "mesh_msgs/msg/mesh_vertex_costs_stamped.hpp" 65 | #include "mesh_msgs/msg/mesh_vertex_tex_coords.hpp" 66 | #include "mesh_msgs/msg/mesh_material.hpp" 67 | #include "mesh_msgs/msg/mesh_texture.hpp" 68 | 69 | #include "sensor_msgs/point_cloud2_iterator.hpp" 70 | 71 | 72 | namespace mesh_msgs_conversions 73 | { 74 | 75 | using Vec = lvr2::BaseVector; 76 | using PointBuffer = lvr2::PointBuffer; 77 | using PointBufferPtr = lvr2::PointBufferPtr; 78 | 79 | struct MaterialGroup 80 | { 81 | int texture_index; 82 | unsigned char r; 83 | unsigned char g; 84 | unsigned char b; 85 | std::vector faceBuffer; 86 | }; 87 | 88 | typedef std::vector > GroupVector; 89 | typedef boost::shared_ptr MaterialGroupPtr; 90 | 91 | template 92 | inline const mesh_msgs::msg::MeshGeometry toMeshGeometry( 93 | const std::shared_ptr>> hem, 94 | const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>()) 95 | { 96 | mesh_msgs::msg::MeshGeometry mesh_msg; 97 | mesh_msg.vertices.reserve(hem->numVertices()); 98 | mesh_msg.faces.reserve(hem->numFaces()); 99 | 100 | mesh_msg.vertex_normals.reserve(normals.numValues()); 101 | 102 | lvr2::DenseVertexMap new_indices; 103 | new_indices.reserve(hem->numVertices()); 104 | 105 | size_t k = 0; 106 | for(auto vH : hem->vertices()) 107 | { 108 | new_indices.insert(vH, k++); 109 | const auto& pi = hem->getVertexPosition(vH); 110 | geometry_msgs::msg::Point p; 111 | p.x = pi.x; p.y = pi.y; p.z = pi.z; 112 | mesh_msg.vertices.push_back(p); 113 | } 114 | 115 | for(auto fH : hem->faces()) 116 | { 117 | mesh_msgs::msg::MeshTriangleIndices indices; 118 | auto vHs = hem->getVerticesOfFace(fH); 119 | indices.vertex_indices[0] = new_indices[vHs[0]]; 120 | indices.vertex_indices[1] = new_indices[vHs[1]]; 121 | indices.vertex_indices[2] = new_indices[vHs[2]]; 122 | mesh_msg.faces.push_back(indices); 123 | } 124 | 125 | for(auto vH : hem->vertices()) 126 | { 127 | const auto& n = normals[vH]; 128 | geometry_msgs::msg::Point v; 129 | v.x = n.x; v.y = n.y; v.z = n.z; 130 | mesh_msg.vertex_normals.push_back(v); 131 | } 132 | 133 | return mesh_msg; 134 | } 135 | 136 | template 137 | inline const mesh_msgs::msg::MeshGeometryStamped toMeshGeometryStamped( 138 | const std::shared_ptr>> hem, 139 | const std::string& frame_id, 140 | const std::string& uuid, 141 | const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>(), 142 | const rclcpp::Time& stamp = rclcpp::Time()) 143 | { 144 | mesh_msgs::msg::MeshGeometryStamped mesh_msg; 145 | mesh_msg.mesh_geometry = toMeshGeometry(hem, normals); 146 | mesh_msg.uuid = uuid; 147 | mesh_msg.header.frame_id = frame_id; 148 | mesh_msg.header.stamp = stamp; 149 | return mesh_msg; 150 | } 151 | 152 | 153 | template 154 | inline const mesh_msgs::msg::MeshGeometry toMeshGeometry( 155 | const lvr2::HalfEdgeMesh>& hem, 156 | const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>()) 157 | { 158 | mesh_msgs::msg::MeshGeometry mesh_msg; 159 | mesh_msg.vertices.reserve(hem.numVertices()); 160 | mesh_msg.faces.reserve(hem.numFaces()); 161 | 162 | mesh_msg.vertex_normals.reserve(normals.numValues()); 163 | 164 | lvr2::DenseVertexMap new_indices; 165 | new_indices.reserve(hem.numVertices()); 166 | 167 | size_t k = 0; 168 | for(auto vH : hem.vertices()) 169 | { 170 | new_indices.insert(vH, k++); 171 | const auto& pi = hem.getVertexPosition(vH); 172 | geometry_msgs::msg::Point p; 173 | p.x = pi.x; p.y = pi.y; p.z = pi.z; 174 | mesh_msg.vertices.push_back(p); 175 | } 176 | 177 | for(auto fH : hem.faces()) 178 | { 179 | mesh_msgs::msg::MeshTriangleIndices indices; 180 | auto vHs = hem.getVerticesOfFace(fH); 181 | indices.vertex_indices[0] = new_indices[vHs[0]]; 182 | indices.vertex_indices[1] = new_indices[vHs[1]]; 183 | indices.vertex_indices[2] = new_indices[vHs[2]]; 184 | mesh_msg.faces.push_back(indices); 185 | } 186 | 187 | for(auto vH : hem.vertices()) 188 | { 189 | const auto& n = normals[vH]; 190 | geometry_msgs::msg::Point v; 191 | v.x = n.x; v.y = n.y; v.z = n.z; 192 | mesh_msg.vertex_normals.push_back(v); 193 | } 194 | 195 | return mesh_msg; 196 | } 197 | 198 | template 199 | inline const mesh_msgs::msg::MeshGeometryStamped toMeshGeometryStamped( 200 | const lvr2::HalfEdgeMesh>& hem, 201 | const std::string& frame_id, 202 | const std::string& uuid, 203 | const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>(), 204 | const rclcpp::Time& stamp = rclcpp::Time()) 205 | { 206 | mesh_msgs::msg::MeshGeometryStamped mesh_msg; 207 | mesh_msg.mesh_geometry = toMeshGeometry(hem, normals); 208 | mesh_msg.uuid = uuid; 209 | mesh_msg.header.frame_id = frame_id; 210 | mesh_msg.header.stamp = stamp; 211 | return mesh_msg; 212 | } 213 | 214 | inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts( 215 | const lvr2::VertexMap& costs, 216 | const size_t num_values, 217 | const float default_value) 218 | { 219 | mesh_msgs::msg::MeshVertexCosts costs_msg; 220 | costs_msg.costs.resize(num_values, default_value); 221 | for(auto vH : costs){ 222 | costs_msg.costs[vH.idx()] = costs[vH]; 223 | } 224 | return costs_msg; 225 | } 226 | 227 | inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped( 228 | const lvr2::VertexMap& costs, 229 | const size_t num_values, 230 | const float default_value, 231 | const std::string& name, 232 | const std::string& frame_id, 233 | const std::string& uuid, 234 | const rclcpp::Time& stamp = rclcpp::Time() 235 | ) 236 | { 237 | mesh_msgs::msg::MeshVertexCostsStamped mesh_msg; 238 | mesh_msg.mesh_vertex_costs = toVertexCosts(costs, num_values, default_value); 239 | mesh_msg.uuid = uuid; 240 | mesh_msg.type = name; 241 | mesh_msg.header.frame_id = frame_id; 242 | mesh_msg.header.stamp = stamp; 243 | return mesh_msg; 244 | } 245 | 246 | inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts( 247 | const lvr2::DenseVertexMap& costs) 248 | { 249 | mesh_msgs::msg::MeshVertexCosts costs_msg; 250 | costs_msg.costs.reserve(costs.numValues()); 251 | for(auto vH : costs){ 252 | costs_msg.costs.push_back(costs[vH]); 253 | } 254 | return costs_msg; 255 | } 256 | 257 | inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped( 258 | const lvr2::DenseVertexMap& costs, 259 | const std::string& name, 260 | const std::string& frame_id, 261 | const std::string& uuid, 262 | const rclcpp::Time& stamp = rclcpp::Time() 263 | ) 264 | { 265 | mesh_msgs::msg::MeshVertexCostsStamped mesh_msg; 266 | mesh_msg.mesh_vertex_costs = toVertexCosts(costs); 267 | mesh_msg.uuid = uuid; 268 | mesh_msg.type = name; 269 | mesh_msg.header.frame_id = frame_id; 270 | mesh_msg.header.stamp = stamp; 271 | return mesh_msg; 272 | } 273 | 274 | 275 | bool fromMeshBufferToMeshGeometryMessage( 276 | const lvr2::MeshBufferPtr& buffer, 277 | mesh_msgs::msg::MeshGeometry& mesh_geometry 278 | ); 279 | 280 | /// Convert lvr2::MeshBuffer to various messages for services 281 | bool fromMeshBufferToMeshMessages( 282 | const lvr2::MeshBufferPtr& buffer, 283 | mesh_msgs::msg::MeshGeometry& mesh_geometry, 284 | mesh_msgs::msg::MeshMaterials& mesh_materials, 285 | mesh_msgs::msg::MeshVertexColors& mesh_vertex_colors, 286 | boost::optional&> texture_cache, 287 | std::string mesh_uuid 288 | ); 289 | 290 | bool fromMeshGeometryToMeshBuffer( 291 | const std::shared_ptr mesh_geometry_ptr, 292 | lvr2::MeshBufferPtr& buffer_ptr 293 | ); 294 | 295 | bool fromMeshGeometryToMeshBuffer( 296 | const mesh_msgs::msg::MeshGeometry& mesh_geometry, 297 | lvr2::MeshBufferPtr& buffer_ptr 298 | ); 299 | 300 | bool fromMeshGeometryToMeshBuffer( 301 | const mesh_msgs::msg::MeshGeometry& mesh_geometry, 302 | lvr2::MeshBuffer& buffer); 303 | 304 | /* TODO 305 | void removeDuplicates(lvr2::MeshBuffer& buffer); 306 | */ 307 | 308 | /** 309 | * @brief Creates a LVR-MeshBufferPointer from a file 310 | * 311 | * @param path Path to a MeshFile 312 | * 313 | * @return LVR-MeshBufferPointer 314 | */ 315 | bool readMeshBuffer(lvr2::MeshBufferPtr& buffer, string path); 316 | 317 | /** 318 | * @brief Writes a LVR-MeshBufferPointer to a file 319 | * 320 | * @param mesh LVR-MeshBufferPointer 321 | * @param path Path to a MeshFile 322 | */ 323 | bool writeMeshBuffer(lvr2::MeshBufferPtr& mesh, string path); 324 | 325 | bool fromPointCloud2ToPointBuffer( 326 | const sensor_msgs::msg::PointCloud2& cloud, PointBuffer& buffer); 327 | 328 | /** 329 | * @brief converts lvr2::Pointbuffer to pointcloud2. 330 | * Every channel is added as a pointfield. 331 | * 332 | * @param buffer the input lvr2::Pointbuffer 333 | * @param frame the frame of the converted pointcloud2 334 | * @param cloud the converted pointcloud2 335 | */ 336 | void PointBufferToPointCloud2( 337 | const lvr2::PointBufferPtr& buffer, 338 | std::string frame, 339 | std::shared_ptr& cloud); 340 | 341 | /** 342 | * @brief converts pointcloud2 to a newly created Pointerbuffer. 343 | * Every pointfield is written into its own channel. 344 | * 345 | * @param cloud the input cloud 346 | * @param buffer the converted lvr2::Pointbuffer 347 | * 348 | * @return 349 | */ 350 | void PointCloud2ToPointBuffer( 351 | const std::shared_ptr cloud, 352 | lvr2::PointBufferPtr& buffer); 353 | 354 | 355 | /** 356 | * @brief Convert mesh_msgs::MeshGeometry to lvr2::MeshBuffer 357 | * @param message to be read 358 | * @param buffer to be returned 359 | * @return bool success status 360 | */ 361 | bool fromMeshGeometryMessageToMeshBuffer( 362 | const mesh_msgs::msg::MeshGeometry& mesh_geometry, 363 | const lvr2::MeshBufferPtr& buffer 364 | ); 365 | 366 | } // end namespace 367 | 368 | #endif /* MESH_MSGS_CONVERSIONS_H_ */ 369 | -------------------------------------------------------------------------------- /mesh_msgs_conversions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mesh_msgs_conversions 5 | 2.0.0 6 | converts point clouds and attributes into meshes and mesh attributes 7 | 8 | Alexander Mock 9 | Matthias Holoch 10 | Sebastian Pütz 11 | 12 | BSD 3-Clause 13 | Sebastian Pütz 14 | Henning Deeken 15 | 16 | ament_cmake 17 | lvr2 18 | rclcpp 19 | sensor_msgs 20 | mesh_msgs 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /mesh_msgs_transform/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_msgs_transform 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.1.0 (2021-10-27) 10 | ------------------ 11 | * removed unused messages 12 | * Contributors: Malte kl. Piening 13 | 14 | 1.0.1 (2020-11-11) 15 | ------------------ 16 | 17 | 1.0.0 (2020-04-26) 18 | ------------------ 19 | * release version 1.0.0 20 | -------------------------------------------------------------------------------- /mesh_msgs_transform/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mesh_msgs_transform) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(mesh_msgs REQUIRED) 16 | find_package(tf2 REQUIRED) 17 | find_package(tf2_ros REQUIRED) 18 | find_package(geometry_msgs REQUIRED) 19 | find_package(Eigen3 REQUIRED) 20 | 21 | include_directories( 22 | include 23 | ${EIGEN3_INCLUDE_DIRS} 24 | ) 25 | 26 | add_library(${PROJECT_NAME} 27 | src/transforms.cpp 28 | ) 29 | 30 | target_include_directories(${PROJECT_NAME} PUBLIC 31 | "$" 32 | "$" 33 | ${EIGEN3_INCLUDE_DIRS} 34 | ) 35 | 36 | target_link_libraries(${PROJECT_NAME} PUBLIC 37 | Eigen3::Eigen 38 | ) 39 | 40 | target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_MSGS_TRANSFORM_BUILDING_LIBRARY")# 41 | 42 | ament_target_dependencies(${PROJECT_NAME} PUBLIC 43 | rclcpp 44 | mesh_msgs 45 | tf2 46 | tf2_ros 47 | geometry_msgs 48 | ) 49 | 50 | # Export old-style CMake variables 51 | ament_export_include_directories("include/${PROJECT_NAME}") 52 | 53 | # Export modern CMake targets 54 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 55 | 56 | ament_export_dependencies( 57 | rclcpp 58 | mesh_msgs 59 | tf2 60 | tf2_ros 61 | geometry_msgs 62 | ) 63 | 64 | install( 65 | TARGETS ${PROJECT_NAME} 66 | EXPORT ${PROJECT_NAME} 67 | ARCHIVE DESTINATION lib 68 | LIBRARY DESTINATION lib 69 | RUNTIME DESTINATION bin 70 | ) 71 | 72 | install( 73 | DIRECTORY include/ 74 | DESTINATION include/${PROJECT_NAME} 75 | ) 76 | 77 | ament_package() -------------------------------------------------------------------------------- /mesh_msgs_transform/README.md: -------------------------------------------------------------------------------- 1 | # mesh_msgs_transform 2 | 3 | Functions for transforming meshes. 4 | 5 | ## ROS2 Port TODOs 6 | 7 | Here is still tf(1) used. Is this package in use? -------------------------------------------------------------------------------- /mesh_msgs_transform/include/mesh_msgs_transform/transforms.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * transforms.h 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | 46 | #ifndef MESH_MSGS_TRANSFORM__TRANSFORMS_H_ 47 | #define MESH_MSGS_TRANSFORM__TRANSFORMS_H_ 48 | 49 | #include 50 | #include 51 | 52 | namespace mesh_msgs_transform 53 | { 54 | 55 | bool transformGeometryMeshNoTime( 56 | const std::string& target_frame, 57 | const mesh_msgs::msg::MeshGeometryStamped& mesh_in, 58 | const std::string& fixed_frame, 59 | mesh_msgs::msg::MeshGeometryStamped& mesh_out, 60 | const tf2_ros::Buffer& tf_buffer 61 | ); 62 | 63 | } // namespace mesh_msgs_transform 64 | 65 | #endif // MESH_MSGS_TRANSFORM__TRANSFORMS_H_ 66 | -------------------------------------------------------------------------------- /mesh_msgs_transform/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mesh_msgs_transform 5 | 2.0.0 6 | Methods to transform mesh_msgs 7 | Sebastian Pütz 8 | BSD-3 9 | http://wiki.ros.org/ros_mesh_tools/mesh_msgs_transform 10 | Alexander Mock 11 | Matthias Holoch 12 | Sebastian Pütz 13 | Sebastian Pütz 14 | 15 | ament_cmake 16 | rclcpp 17 | tf2 18 | tf2_ros 19 | mesh_msgs 20 | geometry_msgs 21 | eigen 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /mesh_msgs_transform/src/transforms.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * mesh_msgs_transforms.cpp 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | #include "mesh_msgs_transform/transforms.h" 46 | #include 47 | #include 48 | #include 49 | 50 | namespace mesh_msgs_transform{ 51 | 52 | inline void vectorTfToEigen( 53 | const tf2::Vector3& tf_vec, 54 | Eigen::Vector3d& eigen_vec) 55 | { 56 | eigen_vec(0) = tf_vec[0]; 57 | eigen_vec(1) = tf_vec[1]; 58 | eigen_vec(2) = tf_vec[2]; 59 | } 60 | 61 | inline void pointMsgToEigen( 62 | const geometry_msgs::msg::Point& gm_point, 63 | Eigen::Vector3d& eigen_point) 64 | { 65 | eigen_point(0) = gm_point.x; 66 | eigen_point(1) = gm_point.y; 67 | eigen_point(2) = gm_point.z; 68 | } 69 | 70 | inline void pointEigenToMsg( 71 | const Eigen::Vector3d& eigen_point, 72 | geometry_msgs::msg::Point& gm_point) 73 | { 74 | gm_point.x = eigen_point(0); 75 | gm_point.y = eigen_point(1); 76 | gm_point.z = eigen_point(2); 77 | } 78 | 79 | bool transformGeometryMeshNoTime( 80 | const std::string& target_frame, 81 | const mesh_msgs::msg::MeshGeometryStamped& mesh_in, 82 | const std::string& fixed_frame, 83 | mesh_msgs::msg::MeshGeometryStamped& mesh_out, 84 | const tf2_ros::Buffer& tf_buffer) 85 | { 86 | geometry_msgs::msg::TransformStamped transform; 87 | try 88 | { 89 | transform = tf_buffer.lookupTransform( 90 | target_frame, tf2::TimePointZero, 91 | mesh_in.header.frame_id, tf2::TimePointZero, 92 | fixed_frame); 93 | } 94 | catch(const tf2::TransformException& ex) 95 | { 96 | RCLCPP_ERROR(rclcpp::get_logger("mesh_msgs_transform"), "%s", ex.what()); 97 | return false; 98 | } 99 | 100 | Eigen::Quaterniond rotation( 101 | transform.transform.rotation.w, 102 | transform.transform.rotation.x, 103 | transform.transform.rotation.y, 104 | transform.transform.rotation.z); 105 | 106 | Eigen::Translation3d translation( 107 | transform.transform.translation.x, 108 | transform.transform.translation.y, 109 | transform.transform.translation.z ); 110 | 111 | Eigen::Affine3d transformation = translation * rotation; 112 | 113 | if(&mesh_in != &mesh_out) 114 | { 115 | mesh_out.header = mesh_in.header; 116 | mesh_out.mesh_geometry.faces = mesh_in.mesh_geometry.faces; 117 | } 118 | 119 | mesh_out.mesh_geometry.vertices.resize(mesh_in.mesh_geometry.vertices.size()); 120 | mesh_out.mesh_geometry.vertex_normals.resize(mesh_in.mesh_geometry.vertex_normals.size()); 121 | 122 | // transform vertices 123 | for(size_t i = 0; i < mesh_in.mesh_geometry.vertices.size(); i++) 124 | { 125 | Eigen::Vector3d in_vec, out_vec; 126 | pointMsgToEigen(mesh_in.mesh_geometry.vertices[i], in_vec); 127 | out_vec = transformation * in_vec; 128 | pointEigenToMsg(out_vec, mesh_out.mesh_geometry.vertices[i]); 129 | } 130 | 131 | // rotate normals 132 | for(size_t i = 0; i < mesh_in.mesh_geometry.vertex_normals.size(); i++) 133 | { 134 | Eigen::Vector3d in_vec, out_vec; 135 | pointMsgToEigen(mesh_in.mesh_geometry.vertex_normals[i], in_vec); 136 | out_vec = transformation.rotation() * in_vec; 137 | pointEigenToMsg(out_vec, mesh_out.mesh_geometry.vertex_normals[i]); 138 | } 139 | 140 | mesh_out.header.frame_id = target_frame; 141 | mesh_out.header.stamp = mesh_in.header.stamp;; 142 | 143 | return true; 144 | } 145 | 146 | } /* namespace mesh_msgs_transforms */ 147 | -------------------------------------------------------------------------------- /mesh_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_mesh_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | 9 | 1.1.0 (2021-10-27) 10 | ------------------ 11 | 12 | 1.0.1 (2020-11-11) 13 | ------------------ 14 | 15 | 1.0.0 (2020-04-26) 16 | ------------------ 17 | * release version 1.0.0 18 | -------------------------------------------------------------------------------- /mesh_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(mesh_tools NONE) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /mesh_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mesh_tools 5 | 2.0.0 6 | The mesh_tools package 7 | Sebastian Pütz 8 | BSD-3 9 | http://wiki.ros.org/mesh_tools 10 | Alexander Mock 11 | Matthias Holoch 12 | Sebastian Pütz 13 | Sebastian Pütz 14 | 15 | ament_cmake 16 | 17 | mesh_msgs 18 | mesh_msgs_transform 19 | mesh_msgs_conversions 20 | rviz_mesh_tools_plugins 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_mesh_tools_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.0.0 (2024-05-15) 6 | ------------------ 7 | * port package to ROS2 8 | * MeshVisual: Add texture support for maps loaded with assimp 9 | 10 | 1.1.0 (2021-10-27) 11 | ------------------ 12 | * fixed GetUUIDs service call in MeshDisplay 13 | * added option to visualize vertex costs using the rviz Map Visuslization 14 | * removed unused messages 15 | * added support for multiple meshes to be visualized at the same time 16 | * added cleaning of generalmesh before adding new data 17 | * removed old plugin and displays 18 | * fixed thin wireframe 19 | * fixed normals shading 20 | * improved normals performance 21 | * fixed rainbow-redgreen cost color type change 22 | * fixed duplicate vertex cost types 23 | * fixed map3d and fixed loading Vertex colors when updating the VertexColors topic 24 | * MeshDisplay: removed visual access from topic and service callbacks 25 | * added option to stop MeshDisplay listening to MeshMsgs 26 | * MeshDisplay now woring with MeshMap 27 | * added properties to MeshDisplay 28 | * added Map3D file dialog property 29 | * added textured mesh display for mesh msgs visualization to rviz_mesh_tools_plugins 30 | * combined mesh-plugin and map-plugin texture mesh visuals 31 | * fixed mpi error 32 | * resolved catkin lint problems 33 | 34 | 1.0.1 (2020-11-11) 35 | ------------------ 36 | 37 | 1.0.0 (2020-04-26) 38 | ------------------ 39 | * release version 1.0.0 40 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(rviz_mesh_tools_plugins) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) 14 | if (NOT CMAKE_BUILD_TYPE) 15 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) 16 | endif() 17 | endif() 18 | 19 | add_definitions(-D_BUILD_DIR_PATH="${CMAKE_CURRENT_BINARY_DIR}") 20 | add_definitions(-D_SRC_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}") 21 | 22 | find_package(ament_cmake REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(rmw REQUIRED) 25 | find_package(rviz_common REQUIRED) 26 | find_package(rviz_rendering REQUIRED) 27 | find_package(rviz_ogre_vendor REQUIRED) 28 | find_package(rviz_assimp_vendor REQUIRED) 29 | find_package(std_msgs REQUIRED) 30 | find_package(mesh_msgs REQUIRED) 31 | find_package(tf2_ros REQUIRED) 32 | find_package(pluginlib REQUIRED) 33 | find_package(message_filters REQUIRED) 34 | find_package(std_msgs REQUIRED) 35 | 36 | find_package(Boost REQUIRED COMPONENTS system filesystem) 37 | find_package(HDF5 REQUIRED COMPONENTS C CXX HL) 38 | # find_package(assimp REQUIRED) 39 | 40 | # include_directories(${ASSIMP_INCLUDE_DIRS}) 41 | 42 | # LVR2 includes HighFive that we need here 43 | # - it was compiling before because globale lvr2 header were accessible 44 | find_package(LVR2 REQUIRED) 45 | 46 | ## This setting causes Qt's "MOC" generation to happen automatically. 47 | # set(CMAKE_AUTOMOC ON) 48 | set(CMAKE_AUTOMOC ON) 49 | find_package(Qt5 COMPONENTS Core Widgets) 50 | # add_definitions(-DQT_NO_KEYWORDS) 51 | 52 | include_directories( 53 | include 54 | ${LVR2_INCLUDE_DIRS} 55 | ${Boost_INCLUDE_DIRS} 56 | ${HDF5_INCLUDE_DIRS} 57 | ) 58 | 59 | set(SOURCE_FILES 60 | src/ClusterLabelDisplay.cpp 61 | src/ClusterLabelPanel.cpp 62 | src/ClusterLabelTool.cpp 63 | src/ClusterLabelVisual.cpp 64 | src/MapDisplay.cpp 65 | src/MeshDisplay.cpp 66 | src/MeshVisual.cpp 67 | src/RvizFileProperty.cpp 68 | src/MeshPoseTool.cpp 69 | src/MeshGoalTool.cpp 70 | src/MeshPoseGuessTool.cpp 71 | src/InteractionHelper.cpp 72 | ) 73 | 74 | 75 | set(MOC_HEADER_FILES 76 | include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp 77 | include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp 78 | include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp 79 | include/rviz_mesh_tools_plugins/MapDisplay.hpp 80 | include/rviz_mesh_tools_plugins/MeshDisplay.hpp 81 | include/rviz_mesh_tools_plugins/MeshVisual.hpp 82 | include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp 83 | include/rviz_mesh_tools_plugins/RvizFileProperty.hpp 84 | include/rviz_mesh_tools_plugins/MeshPoseTool.hpp 85 | include/rviz_mesh_tools_plugins/MeshGoalTool.hpp 86 | include/rviz_mesh_tools_plugins/MeshPoseGuessTool.hpp 87 | include/rviz_mesh_tools_plugins/InteractionHelper.hpp 88 | ) 89 | 90 | add_definitions(${LVR2_DEFINITIONS}) 91 | 92 | add_library(${PROJECT_NAME} SHARED 93 | ${SOURCE_FILES} 94 | ${MOC_HEADER_FILES} 95 | ) 96 | 97 | target_include_directories(${PROJECT_NAME} PUBLIC 98 | $ 99 | $ 100 | ${Qt5Widgets_INCLUDE_DIRS} 101 | ) 102 | 103 | target_link_libraries(${PROJECT_NAME} PUBLIC 104 | ${LVR2_LIBRARIES} 105 | rviz_ogre_vendor::OgreMain 106 | rviz_ogre_vendor::OgreOverlay 107 | Boost::system 108 | Boost::filesystem 109 | ) 110 | 111 | target_link_libraries(${PROJECT_NAME} PRIVATE 112 | ${HDF5_LIBRARIES} 113 | ${HDF5_HL_LIBRARIES} 114 | ) 115 | 116 | target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_MESH_TOOLS_PLUGINS_BUILDING_LIBRARY")# 117 | 118 | pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) 119 | 120 | ament_target_dependencies(${PROJECT_NAME} 121 | PUBLIC 122 | rclcpp 123 | rmw 124 | rviz_common 125 | rviz_rendering 126 | rviz_assimp_vendor 127 | std_msgs 128 | mesh_msgs 129 | tf2_ros 130 | message_filters 131 | std_msgs 132 | ) 133 | 134 | # Export old-style CMake variables 135 | ament_export_include_directories("include/${PROJECT_NAME}") 136 | 137 | # Export modern CMake targets 138 | ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) 139 | 140 | ament_export_dependencies( 141 | rclcpp 142 | rmw 143 | rviz_common 144 | rviz_rendering 145 | rviz_assimp_vendor 146 | std_msgs 147 | mesh_msgs 148 | tf2_ros 149 | message_filters 150 | std_msgs 151 | ) 152 | 153 | install( 154 | TARGETS ${PROJECT_NAME} 155 | EXPORT ${PROJECT_NAME} 156 | ARCHIVE DESTINATION lib 157 | LIBRARY DESTINATION lib 158 | RUNTIME DESTINATION bin 159 | ) 160 | 161 | install( 162 | DIRECTORY include/ 163 | DESTINATION include/${PROJECT_NAME} 164 | ) 165 | 166 | install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" 167 | DESTINATION share/${PROJECT_NAME}) 168 | 169 | ament_package( 170 | CONFIG_EXTRAS "rviz_mesh_tools_plugins-extras.cmake" 171 | ) 172 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/README.md: -------------------------------------------------------------------------------- 1 | Plugin for viewing and labeling HDF5 mesh maps 2 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/icons/classes/ClusterLabel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_tools/7b4f25a3f68b6b2ae8c406be3ae75b4c800266a7/rviz_mesh_tools_plugins/icons/classes/ClusterLabel.png -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/icons/classes/Map3D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_tools/7b4f25a3f68b6b2ae8c406be3ae75b4c800266a7/rviz_mesh_tools_plugins/icons/classes/Map3D.png -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/icons/classes/Mesh.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_tools/7b4f25a3f68b6b2ae8c406be3ae75b4c800266a7/rviz_mesh_tools_plugins/icons/classes/Mesh.png -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/icons/classes/MeshGoal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_tools/7b4f25a3f68b6b2ae8c406be3ae75b4c800266a7/rviz_mesh_tools_plugins/icons/classes/MeshGoal.png -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/icons/classes/MeshPoseGuess.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mesh_tools/7b4f25a3f68b6b2ae8c406be3ae75b4c800266a7/rviz_mesh_tools_plugins/icons/classes/MeshPoseGuess.png -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelDisplay.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | */ 48 | 49 | #ifndef CLUSTER_LABEL_DISPLAY_HPP 50 | #define CLUSTER_LABEL_DISPLAY_HPP 51 | 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | #include 63 | #include 64 | #include 65 | 66 | #include 67 | #include 68 | #include 69 | 70 | // #include 71 | // #include 72 | #include 73 | #include 74 | #include 75 | #include 76 | 77 | #include 78 | #include 79 | 80 | 81 | #include 82 | #include 83 | #include 84 | 85 | 86 | 87 | #ifndef Q_MOC_RUN 88 | #include 89 | 90 | #include 91 | #include 92 | #include 93 | #include 94 | #include 95 | #include 96 | #include 97 | #include 98 | #include 99 | 100 | #include 101 | #include 102 | #include 103 | #include 104 | #include 105 | #include 106 | #include 107 | 108 | #endif // Q_MOC_RUN 109 | 110 | namespace rviz_common 111 | { 112 | namespace properties 113 | { 114 | // Forward declaration 115 | class BoolProperty; 116 | class ColorProperty; 117 | class FloatProperty; 118 | class IntProperty; 119 | class EnumProperty; 120 | class StringProperty; 121 | } // namespace properties 122 | } // namespace rviz_common 123 | 124 | namespace rviz_mesh_tools_plugins 125 | { 126 | using std::map; 127 | using std::shared_ptr; 128 | using std::string; 129 | using std::unique_ptr; 130 | using std::vector; 131 | 132 | // Forward declaration 133 | class ClusterLabelVisual; 134 | class ClusterLabelTool; 135 | 136 | /** 137 | * @class ClusterLabelDisplay 138 | * @brief Display class for the map plugin 139 | */ 140 | class ClusterLabelDisplay : public rviz_common::Display 141 | { 142 | Q_OBJECT 143 | 144 | public: 145 | /** 146 | * @brief Constructor 147 | */ 148 | ClusterLabelDisplay(); 149 | 150 | /** 151 | * @brief Destructor 152 | */ 153 | ~ClusterLabelDisplay(); 154 | 155 | /** 156 | * @brief The tool will call this function and emit the signal below to the master display to 157 | * create the label 158 | * @param label The label name 159 | * @param faces The list of face IDs 160 | */ 161 | void addLabel(string label, vector faces); 162 | 163 | /** 164 | * @brief RViz callback on enable 165 | */ 166 | void onEnable(); 167 | 168 | /** 169 | * @brief RViz callback on disable 170 | */ 171 | void onDisable(); 172 | 173 | Q_SIGNALS: 174 | 175 | /** 176 | * @brief This signal is used for delegating new label data to the master display. 177 | * @param cluster The cluster 178 | */ 179 | void signalAddLabel(Cluster cluster); 180 | 181 | public Q_SLOTS: // not sure wether any of those actually need to be q slots ... 182 | 183 | /** 184 | * @brief Refreshes the tool's current visual 185 | */ 186 | void notifyLabelTool(); 187 | 188 | /** 189 | * @brief Getter for the current geometry 190 | * @return The geometry 191 | */ 192 | shared_ptr getGeometry(); 193 | 194 | /** 195 | * @brief Setter for the geometry and cluster data 196 | * @param geometry The geometry 197 | * @param clusters The clusters 198 | */ 199 | void setData(shared_ptr geometry, vector clusters); 200 | 201 | /** 202 | * @brief Slot for changing the culling mode used in the LabelTool, and the LabelVisuals 203 | */ 204 | void setCullingMode(Ogre::CullingMode mode); 205 | 206 | private Q_SLOTS: 207 | 208 | /** 209 | * @brief Update the map, based on newly loaded data since the last update 210 | */ 211 | void updateMap(); 212 | 213 | /** 214 | * @brief Updates the colors, based on newly loaded data since the last update 215 | */ 216 | void updateColors(); 217 | 218 | /** 219 | * @brief Updates the sphere size for the brush tool 220 | */ 221 | void updateSphereSize(); 222 | 223 | /** 224 | * @brief Updates the phantom visual, based on newly loaded data since the last update 225 | */ 226 | void updatePhantomVisual(); 227 | 228 | /** 229 | * @brief Slot for changing the visual to the selected visual from the dropdown menu 230 | */ 231 | void changeVisual(); 232 | 233 | private: 234 | /** 235 | * @brief RViz callback on initialize 236 | */ 237 | void onInitialize(); 238 | 239 | /** 240 | * @brief Find the instance of the ClusterLabelTool or create one. 241 | * 242 | * We cannot store a pointer to the tool because RViz could sometimes replaces 243 | * the Tool instance, and that would invalidate our pointer. 244 | */ 245 | ClusterLabelTool* getOrCreateLabelTool(); 246 | 247 | /** 248 | * @brief Create visuals for each cluster in the list 249 | */ 250 | void createVisualsFromClusterList(); 251 | 252 | /** 253 | * @brief Creates a phantom visual 254 | */ 255 | void createPhantomVisual(); 256 | 257 | /** 258 | * @brief Dynamically fills the dropdown menus of those properties 259 | */ 260 | void fillPropertyOptions(); 261 | 262 | /// Geometry 263 | shared_ptr m_geometry; 264 | 265 | /// Visuals 266 | vector> m_visuals; 267 | 268 | /// ID of the current active visual 269 | uint32_t m_activeVisualId = 0; 270 | 271 | /// Additional visual to help with labeling without a TexturedMesh 272 | unique_ptr m_phantomVisual; 273 | 274 | /// Cluster data 275 | vector m_clusterList; 276 | 277 | /// Property for the current active visual 278 | rviz_common::properties::EnumProperty* m_activeVisualProperty; 279 | 280 | /// Property to set transparency 281 | rviz_common::properties::FloatProperty* m_alphaProperty; 282 | 283 | /// Property for selecting colors (menu) 284 | rviz_common::properties::Property* m_colorsProperty; 285 | 286 | /// Properties for selecting colors (menu-items) 287 | std::vector m_colorProperties; 288 | 289 | /// Property to set the brushsize of the sphere brush of the label tool from this package 290 | rviz_common::properties::FloatProperty* m_sphereSizeProperty; 291 | 292 | /// Property to hide or show a phantom visual 293 | rviz_common::properties::BoolProperty* m_phantomVisualProperty; 294 | 295 | /// Index for the visuals 296 | int m_labelToolVisualIndex = 0; 297 | 298 | /// A variable that will be set to true, once the initial data has arrived 299 | bool has_data = false; 300 | 301 | /// Current Culling Mode 302 | Ogre::CullingMode m_cullingMode; 303 | 304 | 305 | }; 306 | 307 | } // namespace rviz_mesh_tools_plugins 308 | 309 | #endif // CLUSTER_LABEL_DISPLAY_HPP 310 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelPanel.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | * Alexander Mock 48 | */ 49 | 50 | #ifndef CLUSTER_LABEL_PANEL_HPP 51 | #define CLUSTER_LABEL_PANEL_HPP 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | // Forward declarations 61 | class QLineEdit; 62 | class QPushButton; 63 | 64 | namespace rviz 65 | { 66 | class Tool; 67 | } 68 | 69 | namespace rviz_mesh_tools_plugins 70 | { 71 | /** 72 | * @class ClusterLabelPanel 73 | * @brief Panel for interacting with the label tool 74 | */ 75 | class ClusterLabelPanel : public rviz_common::Panel 76 | { 77 | Q_OBJECT 78 | 79 | public: 80 | /** 81 | * @brief Constructor 82 | * @param parent This panel's parent, if available 83 | */ 84 | ClusterLabelPanel(QWidget* parent = 0); 85 | 86 | /** 87 | * @brief RViz callback on inizialize 88 | */ 89 | void onInitialize(); 90 | 91 | /** 92 | * @brief Load a configuration 93 | * @input config The configuration 94 | */ 95 | virtual void load(const rviz_common::Config& config); 96 | 97 | /** 98 | * @brief Save a configuration 99 | * @input config The configuration 100 | */ 101 | virtual void save(rviz_common::Config config) const; 102 | 103 | public Q_SLOTS: 104 | 105 | /** 106 | * @brief Set the name under which the current cluster will be saved 107 | * @param clusterName The new name 108 | */ 109 | void setClusterName(const QString& clusterName); 110 | 111 | /** 112 | * @brief Updates the current cluster name 113 | */ 114 | void updateClusterName(); 115 | 116 | /** 117 | * @brief Publishes the current cluster 118 | */ 119 | void publish(); 120 | 121 | /** 122 | * @brief Resets the current face selection state 123 | */ 124 | void resetFaces(); 125 | 126 | protected: 127 | /// Input for entering the cluster name 128 | QLineEdit* m_clusterNameEditor; 129 | /// Input for entering the output topic name 130 | QLineEdit* m_outputTopicEditor; 131 | 132 | /// Name of the cluster 133 | QString m_clusterName; 134 | 135 | /// Button for creating and publishing the cluster 136 | QPushButton* m_createClusterButton; 137 | /// Button for resetting the current faces in cluster 138 | QPushButton* m_resetFacesButton; 139 | 140 | /// Instance of the label tool from this package 141 | ClusterLabelTool* m_tool; 142 | 143 | /// Node handle 144 | // ros::NodeHandle m_nodeHandle; 145 | }; 146 | 147 | } // end namespace rviz_mesh_tools_plugins 148 | 149 | #endif -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelTool.hpp 41 | * 42 | * authors: 43 | * Kristin Schmidt 44 | * Jan Philipp Vogtherr 45 | * 46 | */ 47 | 48 | #ifndef CLUSTER_LABEL_TOOL_HPP 49 | #define CLUSTER_LABEL_TOOL_HPP 50 | 51 | 52 | 53 | #include 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | 68 | #include 69 | #include 70 | #include 71 | 72 | #include 73 | #include 74 | #include 75 | 76 | #include 77 | #include 78 | #include 79 | 80 | #include 81 | 82 | #ifndef Q_MOC_RUN 83 | #include 84 | 85 | 86 | #include 87 | #include 88 | #include 89 | #include 90 | #include 91 | #include 92 | #include 93 | #include 94 | 95 | #include 96 | 97 | #endif // Q_MOC_RUN 98 | 99 | #include 100 | 101 | namespace rviz_common 102 | { 103 | namespace properties 104 | { 105 | class RosTopicProperty; 106 | class ColorProperty; 107 | } // namespace properties 108 | } // namespace rviz_common 109 | 110 | // OGRE stuff 111 | namespace Ogre 112 | { 113 | // Forward declaration 114 | // class Vector3; 115 | } // namespace Ogre 116 | 117 | namespace rviz_mesh_tools_plugins 118 | { 119 | // Forward declarations 120 | class ClusterLabelDisplay; 121 | class ClusterLabelVisual; 122 | 123 | /** 124 | * @class ClusterLabelTool 125 | * @brief Tool for selecting faces 126 | */ 127 | class ClusterLabelTool : public rviz_common::Tool 128 | { 129 | Q_OBJECT 130 | public: 131 | // Constants 132 | static constexpr float MIN_BRUSH_SIZE = 20.0f; 133 | static constexpr float MOUSE_WHEEL_BRUSH_SIZE_STEP = 10; 134 | 135 | /** 136 | * @brief Constructor 137 | */ 138 | ClusterLabelTool(); 139 | 140 | /** 141 | * @brief Destructor 142 | */ 143 | virtual ~ClusterLabelTool(); 144 | 145 | /** 146 | * @brief RViz callback on initialize 147 | */ 148 | virtual void onInitialize(); 149 | 150 | /** 151 | * @brief RViz callback for activating 152 | */ 153 | virtual void activate(); 154 | 155 | /** 156 | * @bríef RViz callback for deactivating 157 | */ 158 | virtual void deactivate(); 159 | 160 | /** 161 | * @brief RViz callback for mouse events 162 | * @param event The mouse event 163 | * @return Exit code 164 | */ 165 | virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event); 166 | 167 | /** 168 | * @brief Connects this tool with a given display 169 | * @param display The display that creates this tool 170 | */ 171 | void setDisplay(ClusterLabelDisplay* display); 172 | 173 | /** 174 | * @brief Connects this tool with a given visual 175 | * @param visual The visual that will become editable with this tool 176 | */ 177 | void setVisual(std::shared_ptr visual); 178 | 179 | /** 180 | * @brief Adjust the circle size for the brush tool 181 | * @param size The circle diameter in screen Pixels 182 | */ 183 | void setBrushSize(float size); 184 | 185 | /** 186 | * @brief Set the culling mode for selection to match the MeshVisual 187 | */ 188 | void setCullingMode(Ogre::CullingMode mode); 189 | 190 | public Q_SLOTS: 191 | 192 | /** 193 | * @brief Publish a label with a given namen 194 | * @param name The label name 195 | */ 196 | void publishLabel(std::string name); 197 | 198 | /** 199 | * @brief Returns a list of selected face ID's 200 | * @return List of face ID's 201 | */ 202 | std::vector getSelectedFaces(); 203 | 204 | /** 205 | * @brief Resets the list of selected faces 206 | */ 207 | void resetFaces(); 208 | 209 | /** 210 | * @brief Resets the current visual 211 | */ 212 | void resetVisual(); 213 | 214 | private: 215 | std::vector m_selectedFaces; 216 | std::vector m_faceSelectedArray; 217 | ClusterLabelDisplay* m_display; 218 | std::shared_ptr m_visual; 219 | std::shared_ptr m_meshGeometry; 220 | float m_brushSize; 221 | 222 | // Selection Box 223 | Ogre::SceneNode* m_sceneNode; 224 | Ogre::ManualObject* m_selectionBox; 225 | Ogre::MaterialPtr m_selectionBoxMaterial; 226 | 227 | // Selection Circle 228 | Ogre::ManualObject* m_selectionCircle; 229 | Ogre::SceneNode* m_selectionCircleNode; 230 | 231 | int m_bb_x1; 232 | int m_bb_y1; 233 | int m_bb_x2; 234 | int m_bb_y2; 235 | 236 | rviz_common::RenderPanel* m_evt_panel; 237 | 238 | // Selection Modes 239 | bool m_multipleSelect = false; 240 | bool m_singleSelect = false; 241 | bool m_circleSelect = false; 242 | // Select = true Deselect = false 243 | bool m_selectionMode = false; 244 | 245 | std::vector m_vertexPositions; 246 | 247 | void initSelectionCircle(); 248 | void updateSelectionCircle(rviz_common::ViewportMouseEvent& event); 249 | void updateSelectionBox(); 250 | void selectionBoxStart(rviz_common::ViewportMouseEvent& event); 251 | void selectionBoxMove(rviz_common::ViewportMouseEvent& event); 252 | void selectMultipleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); 253 | void selectSingleFace(rviz_common::ViewportMouseEvent& event, bool selectMode); 254 | void selectCircleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); 255 | 256 | rclcpp::Publisher::SharedPtr m_labelPublisher; 257 | 258 | /** 259 | * @brief Renders the current Mesh to an Offscreen Buffer using the FaceIDs as colors. 260 | * 261 | * The resulting Image can be used to determine which faces are visible from the Camera. 262 | * 263 | * @return The rendered Image. 264 | */ 265 | Ogre::Image renderMeshWithFaceID(); 266 | 267 | /** 268 | * @brief Setup the Selection Mesh from the current geometry 269 | */ 270 | void updateSelectionMesh(); 271 | 272 | // Accelerated area picking via Ogre render pass 273 | Ogre::TexturePtr m_selectionTexture; 274 | Ogre::MaterialPtr m_selectionMaterial; 275 | Ogre::ManualObject* m_selectionMesh; 276 | Ogre::SceneNode* m_selectionSceneNode; 277 | // Used to render only the selectionMesh to the offscreen Texture 278 | uint32_t m_selectionVisibilityBit; 279 | Ogre::CullingMode m_cullingMode; 280 | }; 281 | } // end namespace rviz_mesh_tools_plugins 282 | 283 | #endif 284 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelVisual.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | */ 47 | 48 | #ifndef CLUSTER_LABEL_VISUAL_HPP 49 | #define CLUSTER_LABEL_VISUAL_HPP 50 | 51 | #include 52 | 53 | // #include 54 | 55 | #include 56 | 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | 64 | #include 65 | #include 66 | 67 | namespace Ogre 68 | { 69 | // Forward declaration 70 | class SceneNode; 71 | class Mesh; 72 | } // End namespace Ogre 73 | 74 | namespace rviz_mesh_tools_plugins 75 | { 76 | /** 77 | * @class ClusterLabelVisual 78 | * @brief Visual to show a labeled cluster 79 | */ 80 | class ClusterLabelVisual 81 | { 82 | public: 83 | /** 84 | * @brief Constructor 85 | * 86 | * @param context The context that contains the display information. 87 | * @param labelId The label id (that has to be unique) 88 | */ 89 | // ClusterLabelVisual( 90 | // rviz_common::DisplayContext* context, 91 | // std::string labelId); 92 | 93 | /** 94 | * @brief Constructor 95 | * 96 | * @param context The context that contains the display information. 97 | * @param labelId The label id (that has to be unique) 98 | * @param geometry A shared pointer to the geometry to which the labels belong 99 | */ 100 | ClusterLabelVisual( 101 | rviz_common::DisplayContext* context, 102 | std::string labelId, 103 | std::shared_ptr geometry); 104 | 105 | /** 106 | * @brief Destructor 107 | */ 108 | virtual ~ClusterLabelVisual(); 109 | 110 | /** 111 | * @brief Disabling the copy constructor 112 | * 113 | * Each cluster label visual has a pointer to a SubMesh with a unique name, 114 | * when copying and then deleting one of the copies, the SubMesh would be deleted, thus the 115 | * pointers of the remaining copies would be invalid 116 | */ 117 | ClusterLabelVisual(const ClusterLabelVisual&) = delete; 118 | 119 | /** 120 | * @brief Disabling the copy assignment operator 121 | * 122 | * explanation: see deleted copy constructor ClusterLabelVisual(const ClusterLabelVisual&) 123 | */ 124 | ClusterLabelVisual& operator=(const ClusterLabelVisual&) = delete; 125 | 126 | /** 127 | * @brief Deletes the material 128 | */ 129 | void reset(); 130 | 131 | /** 132 | * @brief Sets the geometry 133 | * 134 | * @param geometry The geometry 135 | */ 136 | void setGeometry(std::shared_ptr geometry); 137 | 138 | /** 139 | * @brief Sets the faces, that are in the shown cluster 140 | * 141 | * @param faces A vector containing the face ids 142 | */ 143 | void setFacesInCluster(const std::vector& faces); 144 | 145 | /** 146 | * @brief Sets the color 147 | * 148 | * @param facesColor The color for the faces 149 | * @param alpha The opacity, defaults to 1.0f (fully opaque) 150 | */ 151 | void setColor(Ogre::ColourValue facesColor, float alpha = 1.0f); 152 | 153 | /** 154 | * @brief Returns the faces 155 | * 156 | * @return A vector containing the face ids 157 | */ 158 | std::vector getFaces() 159 | { 160 | return m_faces; 161 | }; 162 | 163 | /** 164 | * @brief Set the culling mode to match the MeshDisplay/MeshVisual 165 | */ 166 | void setCullingMode(Ogre::CullingMode mode); 167 | 168 | private: 169 | void initMaterial(); 170 | 171 | rviz_common::DisplayContext* m_displayContext; 172 | Ogre::SceneNode* m_sceneNode; 173 | std::string m_labelId; 174 | 175 | Ogre::MeshPtr m_mesh; 176 | Ogre::SubMesh* m_subMesh; 177 | Ogre::MaterialPtr m_material; 178 | 179 | Ogre::ColourValue m_color; 180 | 181 | std::shared_ptr m_geometry; 182 | std::vector m_faces; 183 | }; 184 | 185 | } // end namespace rviz_mesh_tools_plugins 186 | 187 | #endif 188 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP 2 | #define RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP 3 | 4 | #include 5 | #include 6 | 7 | 8 | namespace rviz_common 9 | { 10 | class DisplayContext; 11 | } // namespace rviz_common 12 | 13 | namespace rviz_mesh_tools_plugins 14 | { 15 | 16 | struct Intersection { 17 | double range; 18 | Ogre::Vector3 point; 19 | Ogre::Vector3 normal; 20 | uint32_t face_id; 21 | uint32_t geom_id; // currently unused 22 | uint32_t inst_id; // currently unused 23 | }; 24 | 25 | Ogre::Ray getMouseEventRay( 26 | const rviz_common::ViewportMouseEvent& event); 27 | 28 | bool selectFace( 29 | const Ogre::ManualObject* mesh, 30 | const Ogre::Ray& ray, 31 | Intersection& intersection); 32 | 33 | bool selectFace( 34 | const rviz_common::DisplayContext* ctx, 35 | const Ogre::Ray& ray, 36 | Intersection& intersection); 37 | 38 | } // namespace rviz_mesh_tools_plugins 39 | 40 | 41 | #endif // RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MapDisplay.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | */ 48 | 49 | #ifndef MAP_DISPLAY_HPP 50 | #define MAP_DISPLAY_HPP 51 | 52 | #include 53 | #include 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | #include 68 | #include 69 | #include 70 | #include 71 | 72 | #include 73 | #include 74 | // #include 75 | // #include 76 | 77 | #include 78 | #include 79 | #include 80 | #include 81 | 82 | #include 83 | #include 84 | #include 85 | 86 | #include 87 | #include 88 | #include 89 | #include 90 | #include 91 | #include 92 | #include 93 | 94 | #ifndef Q_MOC_RUN 95 | #include 96 | 97 | #include 98 | #include 99 | #include 100 | #include 101 | #include 102 | #include 103 | #include 104 | #include 105 | #include 106 | 107 | #endif // Q_MOCK_RUN 108 | 109 | #include 110 | #include 111 | 112 | namespace rviz 113 | { 114 | namespace properties 115 | { 116 | // Forward declaration 117 | class BoolProperty; 118 | class ColorProperty; 119 | class FloatProperty; 120 | class IntProperty; 121 | class EnumProperty; 122 | class StringProperty; 123 | class TfFrameProperty; 124 | } // namespace properties 125 | } // namespace rviz 126 | 127 | namespace rviz_mesh_tools_plugins 128 | { 129 | using std::shared_ptr; 130 | using std::string; 131 | using std::unique_ptr; 132 | using std::vector; 133 | 134 | /** 135 | * @class MapDisplay 136 | * @brief Master display for the Mesh- and Cluster- subdisplays. THis implementation uses HDF5 as it's data source 137 | */ 138 | class MapDisplay : public rviz_common::Display 139 | { 140 | Q_OBJECT 141 | 142 | public: 143 | using Base = rviz_common::Display; 144 | /** 145 | * @brief Constructor 146 | */ 147 | MapDisplay(); 148 | 149 | /** 150 | * @brief Destructor 151 | */ 152 | ~MapDisplay(); 153 | 154 | virtual void load(const rviz_common::Config& config) override; 155 | 156 | public Q_SLOTS: 157 | 158 | /** 159 | * @brief Saves a label to HDF5 160 | * @param cluster The cluster to be saved 161 | */ 162 | void saveLabel(Cluster cluster); 163 | 164 | /** 165 | * @brief Get the geometry 166 | * @return The geometry 167 | */ 168 | shared_ptr getGeometry(); 169 | 170 | private Q_SLOTS: 171 | 172 | /** 173 | * @brief Update the map, based on the current data state 174 | */ 175 | void updateMap(); 176 | 177 | void updateMapFrame(); 178 | 179 | private: 180 | void enableClusterLabelDisplay(); 181 | void disableClusterLabelDisplay(); 182 | void enableMeshDisplay(); 183 | void disableMeshDisplay(); 184 | /** 185 | * @brief RViz callback on initialize 186 | */ 187 | virtual void onInitialize() override; 188 | 189 | /** 190 | * @brief Periodically called from rviz 191 | */ 192 | void update(float wall_dt, float ros_dt) override; 193 | 194 | /** 195 | * @brief RViz callback on enable 196 | */ 197 | virtual void onEnable() override; 198 | 199 | /** 200 | * @brief RViz callback on disable 201 | */ 202 | virtual void onDisable() override; 203 | 204 | /** 205 | * @brief Read all data from the HDF5 file and save it in the member variables 206 | * @return true, if successful 207 | */ 208 | bool loadData(); 209 | 210 | // TODO: make more efficient - currently everything is stored in the MapDisplay, the MeshDisplay and the MeshVisual 211 | /// Geometry 212 | shared_ptr m_geometry; 213 | /// Materials 214 | vector m_materials; 215 | /// Textures 216 | vector m_textures; 217 | /// Colors 218 | vector m_colors; 219 | /// Vertex normals 220 | vector m_normals; 221 | /// Texture coordinates 222 | vector m_texCoords; 223 | /// Clusters 224 | vector m_clusterList; 225 | 226 | std::map> m_costs; 227 | 228 | /// Path to map file 229 | rviz_common::properties::FileProperty* m_mapFilePath; 230 | std::string m_map_file_loaded; 231 | 232 | /// Set the TF Frame of the map 233 | rviz_common::properties::TfFrameProperty* m_mapTfFrame; 234 | 235 | /// Subdisplay: ClusterLabel (for showing the clusters) 236 | rviz_mesh_tools_plugins::ClusterLabelDisplay* m_clusterLabelDisplay; 237 | /// Subdisplay: MeshDisplay (for showing the mesh) 238 | rviz_mesh_tools_plugins::MeshDisplay* m_meshDisplay; 239 | 240 | /** 241 | * @brief Create a RViz display from it's unique class_id 242 | * @param class_id The class ID 243 | * @return Pointer to RViz display 244 | */ 245 | rviz_common::Display* createDisplay(const QString& class_id); 246 | }; 247 | 248 | } // end namespace rviz_mesh_tools_plugins 249 | 250 | #endif 251 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshGoalTool.hpp 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | #ifndef MESH_GOAL_TOOL_HPP 46 | #define MESH_GOAL_TOOL_HPP 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #ifndef Q_MOC_RUN 55 | #include 56 | #endif // Q_MOC_RUN 57 | 58 | #include 59 | 60 | namespace rviz_mesh_tools_plugins 61 | { 62 | /** 63 | * @class MeshGoalTool 64 | * @brief Tool for publishing a goal within a mesh 65 | */ 66 | class MeshGoalTool : public MeshPoseTool 67 | { 68 | Q_OBJECT 69 | public: 70 | /** 71 | * @brief Constructor 72 | */ 73 | MeshGoalTool(); 74 | 75 | /** 76 | * @brief Callback that is executed when tool is initialized 77 | */ 78 | virtual void onInitialize(); 79 | 80 | private Q_SLOTS: 81 | 82 | /** 83 | * @brief Updates the topic on which the goal will be published 84 | */ 85 | void updateTopic(); 86 | 87 | protected: 88 | /** 89 | * @brief When goal is set, publish result 90 | * @param position Position 91 | * @param orientation Orientation 92 | */ 93 | virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); 94 | 95 | /// Property for the topic 96 | rviz_common::properties::StringProperty* topic_property_; 97 | /// Switch bottom / top for selection 98 | rviz_common::properties::BoolProperty* switch_bottom_top_; 99 | /// Publisher 100 | rclcpp::Publisher::SharedPtr pose_pub_; 101 | /// Node handle 102 | // ros::NodeHandle nh_; 103 | }; 104 | 105 | } // namespace rviz_mesh_tools_plugins 106 | 107 | #endif // MESH_GOAL_TOOL_HPP 108 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseGuessTool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2023, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshPoseGuessTool.hpp 41 | * 42 | * author: Alexander Mock 43 | */ 44 | 45 | #ifndef MESH_POSE_ESTIMATE_TOOL_HPP 46 | #define MESH_POSE_ESTIMATE_TOOL_HPP 47 | 48 | #include 49 | #include 50 | #include 51 | 52 | 53 | #ifndef Q_MOC_RUN 54 | #include 55 | #endif // Q_MOC_RUN 56 | 57 | #include 58 | #include "rclcpp/qos.hpp" 59 | 60 | namespace rviz_common 61 | { 62 | 63 | namespace properties 64 | { 65 | 66 | class StringProperty; 67 | class QosProfileProperty; 68 | class BoolProperty; 69 | class FloatProperty; 70 | 71 | } // namespace properties 72 | 73 | } // namespace rviz_common 74 | 75 | namespace rviz_mesh_tools_plugins 76 | { 77 | /** 78 | * @class MeshPoseGuessTool 79 | * @brief Tool for publishing a pose estimate within a mesh 80 | */ 81 | class MeshPoseGuessTool : public MeshPoseTool 82 | { 83 | Q_OBJECT 84 | public: 85 | /** 86 | * @brief Constructor 87 | */ 88 | MeshPoseGuessTool(); 89 | 90 | /** 91 | * @brief Callback that is executed when tool is initialized 92 | */ 93 | virtual void onInitialize(); 94 | 95 | private Q_SLOTS: 96 | 97 | /** 98 | * @brief Updates the topic on which the goal will be published 99 | */ 100 | void updateTopic(); 101 | 102 | protected: 103 | /** 104 | * @brief When goal is set, publish result 105 | * @param position Position 106 | * @param orientation Orientation 107 | */ 108 | virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); 109 | 110 | /// Property for the topic name 111 | rviz_common::properties::StringProperty* topic_property_; 112 | 113 | // Properties for QosProfile 114 | rviz_common::properties::QosProfileProperty* qos_profile_property_; 115 | 116 | /// Switch bottom / top for selection 117 | rviz_common::properties::BoolProperty* switch_bottom_top_; 118 | 119 | // Covariance? 120 | rviz_common::properties::FloatProperty* cov_position_x_property_; 121 | rviz_common::properties::FloatProperty* cov_position_y_property_; 122 | rviz_common::properties::FloatProperty* cov_position_z_property_; 123 | rviz_common::properties::FloatProperty* cov_orientation_x_property_; 124 | rviz_common::properties::FloatProperty* cov_orientation_y_property_; 125 | rviz_common::properties::FloatProperty* cov_orientation_z_property_; 126 | 127 | /// Publisher 128 | rclcpp::Publisher::SharedPtr pose_pub_; 129 | rclcpp::QoS qos_profile_; 130 | rclcpp::Clock::SharedPtr clock_; 131 | }; 132 | 133 | } // namespace rviz_mesh_tools_plugins 134 | 135 | #endif // MESH_POSE_ESTIMATE_TOOL_HPP 136 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshPoseTool.hpp 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | #ifndef MESH_POSE_TOOL_HPP 46 | #define MESH_POSE_TOOL_HPP 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | // #include 56 | 57 | // Forward declare types 58 | namespace rviz_rendering 59 | { 60 | class Arrow; 61 | } 62 | 63 | namespace rviz_mesh_tools_plugins 64 | { 65 | class MeshPoseTool : public rviz_common::Tool 66 | { 67 | public: 68 | MeshPoseTool(); 69 | virtual ~MeshPoseTool(); 70 | 71 | virtual void onInitialize(); 72 | 73 | virtual void activate(); 74 | virtual void deactivate(); 75 | 76 | virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event); 77 | 78 | protected: 79 | 80 | void setColor(float r, float g, float b, float a); 81 | 82 | virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) = 0; 83 | 84 | rviz_rendering::Arrow* arrow_; 85 | enum State 86 | { 87 | Position, 88 | Orientation 89 | }; 90 | State state_; 91 | Ogre::Vector3 pos_start_; 92 | Ogre::Vector3 normal_start_; 93 | Ogre::Vector3 pos_last_; 94 | // smooth normal in pixel coordinates > 2 95 | unsigned smooth_normal_width_ = 9; 96 | }; 97 | 98 | } /* namespace rviz_mesh_tools_plugins */ 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshVisual.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Sebastian Pütz 46 | * Henning Deeken 47 | * Marcel Mrozinski 48 | * Nils Oesting 49 | * Kristin Schmidt 50 | * Jan Philipp Vogtherr 51 | */ 52 | 53 | #ifndef MESH_VISUAL_HPP 54 | #define MESH_VISUAL_HPP 55 | 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | #include 68 | 69 | #include 70 | #include 71 | 72 | #include 73 | #include 74 | #include 75 | #include 76 | #include 77 | #include 78 | 79 | #include 80 | #include 81 | 82 | namespace Ogre 83 | { 84 | // Forward declaration 85 | class Quaternion; 86 | class SceneNode; 87 | class Entity; 88 | 89 | } // End namespace Ogre 90 | 91 | namespace rviz_mesh_tools_plugins 92 | { 93 | /** 94 | * @brief Class to display mesh data in the main panel of rviz. 95 | */ 96 | class MeshVisual 97 | { 98 | public: 99 | /** 100 | * @brief Constructor. 101 | * 102 | * @param context The context that contains the display information. 103 | * @param displayID The display id 104 | * @param meshID The mesh id 105 | * @param randomID random number that will be used as part of the meshes UID 106 | */ 107 | MeshVisual(rviz_common::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID); 108 | 109 | /** 110 | * @brief Destructor. 111 | */ 112 | virtual ~MeshVisual(); 113 | 114 | /** 115 | * @brief Clears whole stored data. 116 | */ 117 | void reset(); 118 | 119 | void hide(); 120 | 121 | void show(); 122 | 123 | /** 124 | * @brief Extracts data from the ros-messages and creates meshes. 125 | * 126 | * @param geometry Geometry containing the mesh 127 | */ 128 | bool setGeometry(const Geometry& geometry); 129 | 130 | /** 131 | * @brief Passes the normal data to the mesh visual 132 | * 133 | * @param normals Vector containing the normal data 134 | */ 135 | bool setNormals(const std::vector& normals); 136 | 137 | /** 138 | * @brief Extracts data from the ros-messages and creates a colored mesh. 139 | * 140 | * @param vertexColors Vector containing the vertex color information 141 | */ 142 | bool setVertexColors(const std::vector& vertexColors); 143 | 144 | /** 145 | * @brief Extracts data from the ros-messages and creates a colored mesh with colors calculated from vertex costs. 146 | * 147 | * @param vertexCosts Vector containing the vertex cost information 148 | */ 149 | bool setVertexCosts(const std::vector& vertexCosts); 150 | 151 | /** 152 | * @brief Extracts data from the ros-messages and creates a colored mesh with colors calculated from vertex costs. 153 | * 154 | * @param vertexCosts Vector containing the vertex cost information 155 | * @param costColorType colorization method (0 = rainbow; 1 = red-green) 156 | */ 157 | bool setVertexCosts(const std::vector& vertexCosts, int costColorType); 158 | 159 | /** 160 | * @brief Extracts data from the ros-messages and creates a colored mesh with colors calculated from vertex costs. 161 | * 162 | * @param vertexCosts Vector containing the vertex cost information 163 | * @param costColorType colorization method (0 = rainbow; 1 = red-green) 164 | * @param minCost minimum value for colorization 165 | * @param maxCost maximum value for colorization 166 | */ 167 | bool setVertexCosts(const std::vector& vertexCosts, int costColorType, float minCost, float maxCost); 168 | 169 | /** 170 | * @brief Extracts data from the ros-messages and creates a textured mesh. 171 | * 172 | * @param materials Vector containing all materials 173 | * @param texCoords Vector containing all texture coordinates 174 | */ 175 | bool setMaterials(const vector& materials, const vector& texCoords); 176 | 177 | /** 178 | * @brief Extracts data from the ros-messages and adds textures to the textured mesh. 179 | * 180 | * @param texture Texture containing the texture information and data 181 | * @param textureIndex Index of the texture 182 | */ 183 | bool addTexture(Texture& texture, uint32_t textureIndex); 184 | 185 | /** 186 | * @brief Sets the pose of the coordinate frame the message refers to. 187 | * 188 | * @param position The pose of the coordinate frame 189 | */ 190 | void setFramePosition(const Ogre::Vector3& position); 191 | 192 | /** 193 | * @brief Sets the orientation of the coordinate frame the message refers to. 194 | * 195 | * @param orientation The orientation of the coordinate frame 196 | */ 197 | void setFrameOrientation(const Ogre::Quaternion& orientation); 198 | 199 | /** 200 | * @brief Sets the hardware culling mode of the mesh. 201 | * 202 | * @param mode The hardware culling mode 203 | */ 204 | void setCullingMode(Ogre::CullingMode mode); 205 | 206 | /** 207 | * @brief Updates the visible parts of the mesh depending on input from the rviz display. 208 | * 209 | * @param showFaces When TRUE faces are visible 210 | * @param facesColor The color of the faces 211 | * @param facesAlpha The transparency of the faces 212 | * @param useVertexColors When TRUE vertex colors are used 213 | * @param showVertexCosts When TRUE vertex costs are visible 214 | * @param showTextures When TRUE textures are visible 215 | * @param showTexturedFacesOnly When TRUE only textured faces are visible 216 | */ 217 | void updateMaterial(bool showFaces, Ogre::ColourValue facesColor, float facesAlpha, bool useVertexColors, 218 | bool showVertexCosts, bool showTextures, bool showTexturedFacesOnly); 219 | 220 | /** 221 | * @brief Updates the visible parts of the mesh depending on input from the rviz display. 222 | * 223 | * @param showWireframe When TRUE wireframe is visible 224 | * @param wireframeColor The color of the wireframe 225 | * @param wireframeAlpha The transparency of the wireframe 226 | * @param showFaces When TRUE faces are visible 227 | * @param facesColor The color of the faces 228 | * @param facesAlpha The transparency of the faces 229 | * @param useVertexColors When TRUE vertex colors are used 230 | * @param showVertexCosts When TRUE vertex costs are visible 231 | * @param showTextures When TRUE textures are visible 232 | * @param showTexturedFacesOnly When TRUE only textured faces are visible 233 | * @param showNormals When TRUE normals are visible 234 | * @param normalsColor The color of the normals 235 | * @param normalsAlpha The transparency of the normals 236 | * @param normalsScallingFactor The size of the normals 237 | */ 238 | void updateMaterial(bool showWireframe, Ogre::ColourValue wireframeColor, float wireframeAlpha, bool showFaces, 239 | Ogre::ColourValue facesColor, float facesAlpha, bool useVertexColors, bool showVertexCosts, 240 | bool showTextures, bool showTexturedFacesOnly, bool showNormals, Ogre::ColourValue normalsColor, 241 | float normalsAlpha, float normalsScallingFactor); 242 | 243 | /** 244 | * @brief Updates the size of the normals dynamically. 245 | * 246 | * @param scallingFactor The factor the normals have to be scaled with 247 | */ 248 | void updateNormals(float scallingFactor); 249 | 250 | /** 251 | * @brief Updates the normals dynamically. 252 | * 253 | * @param showNormals When TRUE normals are visible 254 | * @param normalsColor The color of the normals 255 | * @param normalsAlpha The transparency of the normals 256 | */ 257 | void updateNormals(bool showNormals, Ogre::ColourValue normalsColor, float normalsAlpha); 258 | 259 | /** 260 | * @brief Updates the normals dynamically. 261 | * 262 | * @param showNormals When TRUE normals are visible 263 | * @param normalsColor The color of the normals 264 | * @param normalsAlpha The transparency of the normals 265 | * @param scalingFactor The factor the normals have to be scaled with 266 | */ 267 | void updateNormals(bool showNormals, Ogre::ColourValue normalsColor, float normalsAlpha, float scalingFactor); 268 | 269 | /** 270 | * @brief Updates the wireframe dynamically. 271 | * 272 | * @param showWireframe When TRUE wireframe is visible 273 | * @param wireframeColor The color of the wireframe 274 | * @param wireframeAlpha The transparency of the wireframe 275 | */ 276 | void updateWireframe(bool showWireframe, Ogre::ColourValue wireframeColor, float wireframeAlpha); 277 | 278 | private: 279 | /** 280 | * @brief Enables the wireframe 281 | * @param pass Ogre Pass 282 | * @param wireframeColor The color of the wireframe 283 | * @param wireframeAlpha Transparency of the wireframe 284 | */ 285 | void showWireframe(Ogre::Pass* pass, Ogre::ColourValue wireframeColor, float wireframeAlpha); 286 | 287 | /** 288 | * @brief 289 | */ 290 | void showFaces(Ogre::Pass* pass, Ogre::ColourValue facesColor, float facesAlpha, bool useVertexColors); 291 | 292 | void showNormals(Ogre::Pass* pass, Ogre::ColourValue normalsColor, float normalsAlpha); 293 | 294 | void showTextures(Ogre::Pass* pass); 295 | 296 | void enteringGeneralTriangleMesh(const Geometry& mesh); 297 | 298 | void enteringColoredTriangleMesh(const Geometry& mesh, const vector& vertexColors); 299 | 300 | void enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const vector& vertexCosts, int costColorType); 301 | void enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const vector& vertexCosts, int costColorType, 302 | float minCost, float maxCost); 303 | 304 | void enteringTexturedTriangleMesh(const Geometry& mesh, const vector& meshMaterials, 305 | const vector& texCoords); 306 | 307 | void enteringNormals(const Geometry& mesh, const vector& normals); 308 | 309 | Ogre::PixelFormat getOgrePixelFormatFromRosString(std::string encoding); 310 | 311 | void loadImageIntoTextureMaterial(size_t textureIndex); 312 | 313 | /** 314 | * 315 | * @brief Calculates a color for a given cost value using a spectrum from red to green. 316 | * 317 | * @param cost The cost value (should be within the range 0 - 1) 318 | * 319 | * @return calculated color 320 | */ 321 | Ogre::ColourValue calculateColorFromCost(float cost, int costColorType); 322 | 323 | bool m_vertex_normals_enabled; 324 | bool m_vertex_colors_enabled; 325 | bool m_vertex_costs_enabled; 326 | bool m_materials_enabled; 327 | bool m_texture_coords_enabled; 328 | bool m_textures_enabled; 329 | 330 | /// Ogre Scenenode 331 | Ogre::SceneNode* m_sceneNode; 332 | 333 | /// The context that contains the display information. 334 | rviz_common::DisplayContext* m_displayContext; 335 | 336 | /// First ID of the created mesh 337 | size_t m_prefix; 338 | 339 | /// Second ID of the created mesh 340 | size_t m_postfix; 341 | 342 | /// Random ID of the created mesh 343 | size_t m_random; 344 | 345 | /// The mesh-object to display 346 | Ogre::ManualObject* m_mesh; 347 | bool m_mesh_hidden = false; 348 | 349 | /// The manual object to display normals 350 | Ogre::ManualObject* m_normals; 351 | bool m_normals_hidden = false; 352 | 353 | /// The manual object to display the mesh with vertex costs 354 | Ogre::ManualObject* m_vertexCostsMesh; 355 | bool m_vertexCostsMesh_hidden = false; 356 | 357 | /// The manual object to display the textured mesh 358 | Ogre::ManualObject* m_texturedMesh; 359 | bool m_texturedMesh_hidden = false; 360 | 361 | /// The manual object to display the not textured parts of the textured mesh 362 | Ogre::ManualObject* m_noTexCluMesh; 363 | bool m_noTexCluMesh_hidden = false; 364 | 365 | // The textures for the mesh 366 | std::vector m_images; 367 | 368 | // The material for the textured mesh 369 | Ogre::MaterialPtr m_texturedMeshMaterial; 370 | 371 | /// The material for the general mesh 372 | Ogre::MaterialPtr m_meshGeneralMaterial; 373 | 374 | /// The material for the textured triangle mesh 375 | Ogre::MaterialPtr m_meshTexturedTrianglesMaterial; 376 | 377 | /// The material of the normals 378 | Ogre::MaterialPtr m_normalMaterial; 379 | 380 | /// The materials of the not textured clusters 381 | Ogre::MaterialPtr m_noTexCluMaterial; 382 | 383 | /// The material of the mesh with vertex costs 384 | Ogre::MaterialPtr m_vertexCostMaterial; 385 | 386 | /// The materials of the textures 387 | std::vector m_textureMaterials; 388 | 389 | /// Factor the normal-size is multiplied with. 390 | float m_normalsScalingFactor; 391 | 392 | /// Hardware culling mode 393 | Ogre::CullingMode m_cullingMode; 394 | 395 | /// raw Triangle Mesh 396 | Geometry m_geometry; 397 | 398 | /// raw normals 399 | std::vector m_geometryNormals; 400 | }; 401 | } // End namespace rviz_mesh_tools_plugins 402 | 403 | #endif 404 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP 2 | #define RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace tf2_ros 8 | { 9 | 10 | template 11 | using RVizMessageFilter = tf2_ros::MessageFilter; 12 | 13 | template 14 | using RVizMessageFilterPtr = std::shared_ptr >; 15 | 16 | } // namespace tf2_ros 17 | 18 | #endif // RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2021, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * RvizFileProperty.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Malte kleine Piening 46 | * Alexander Mock 47 | */ 48 | 49 | #ifndef RVIZ_FILE_PROPERTY_HPP 50 | #define RVIZ_FILE_PROPERTY_HPP 51 | 52 | #include 53 | #include 54 | 55 | namespace rviz_common 56 | { 57 | 58 | namespace properties 59 | { 60 | 61 | class FileProperty : public FilePickerProperty 62 | { 63 | Q_OBJECT 64 | public: 65 | FileProperty( 66 | const QString& name = QString(), 67 | const QString& default_value = QString(), 68 | const QString& description = QString(), 69 | Property* parent = nullptr, 70 | const char* changed_slot = nullptr, 71 | QObject* receiver = nullptr); 72 | 73 | std::string getFilename() 74 | { 75 | return getValue().toString().toStdString(); 76 | } 77 | 78 | QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem&); 79 | 80 | public Q_SLOTS: 81 | bool setFilename(const QString& str) 82 | { 83 | return setValue(str); 84 | } 85 | bool setStdFilename(const std::string& std_str) 86 | { 87 | return setValue(QString::fromStdString(std_str)); 88 | } 89 | }; 90 | 91 | } // namespace properties 92 | } // namespace rviz 93 | 94 | #endif // RVIZ_FILE_PROPERTY_HPP 95 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * Types.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | * Alexander Mock 48 | */ 49 | 50 | #pragma once 51 | 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | namespace rviz_mesh_tools_plugins 58 | { 59 | using boost::optional; 60 | using std::array; 61 | using std::string; 62 | using std::vector; 63 | 64 | 65 | 66 | /// Struct for normals 67 | struct Normal 68 | { 69 | float x; 70 | float y; 71 | float z; 72 | 73 | Normal(float _x, float _y, float _z) : x(_x), y(_y), z(_z) 74 | { 75 | } 76 | }; 77 | 78 | 79 | 80 | /// Struct for texture coordinates 81 | struct TexCoords 82 | { 83 | float u; 84 | float v; 85 | 86 | TexCoords(float _u, float _v) : u(_u), v(_v) 87 | { 88 | } 89 | }; 90 | 91 | /// Struct for clusters 92 | struct Cluster 93 | { 94 | string name; 95 | vector faces; 96 | 97 | Cluster(string n, vector f) : name(n), faces(f) 98 | { 99 | } 100 | }; 101 | 102 | /// Struct for vertices 103 | struct Vertex 104 | { 105 | float x; 106 | float y; 107 | float z; 108 | }; 109 | 110 | /// Struct for faces 111 | struct Face 112 | { 113 | array vertexIndices; 114 | }; 115 | 116 | /// Struct for geometry 117 | struct Geometry 118 | { 119 | vector vertices; 120 | vector faces; 121 | 122 | Geometry() 123 | { 124 | } 125 | 126 | Geometry(vector v, vector f) 127 | { 128 | for (uint32_t i = 0; i < v.size() / 3; i++) 129 | { 130 | Vertex vertex; 131 | vertex.x = v[i * 3 + 0]; 132 | vertex.y = v[i * 3 + 1]; 133 | vertex.z = v[i * 3 + 2]; 134 | vertices.push_back(vertex); 135 | } 136 | 137 | for (uint32_t i = 0; i < f.size() / 3; i++) 138 | { 139 | Face face; 140 | face.vertexIndices[0] = f[i * 3 + 0]; 141 | face.vertexIndices[1] = f[i * 3 + 1]; 142 | face.vertexIndices[2] = f[i * 3 + 2]; 143 | faces.push_back(face); 144 | } 145 | } 146 | }; 147 | 148 | /// Struct for colors 149 | struct Color 150 | { 151 | float r; 152 | float g; 153 | float b; 154 | float a; 155 | 156 | Color() 157 | { 158 | } 159 | Color(float _r, float _g, float _b, float _a) : r(_r), g(_g), b(_b), a(_a) 160 | { 161 | } 162 | }; 163 | 164 | /// Struct for textures 165 | struct Texture 166 | { 167 | uint32_t width; 168 | uint32_t height; 169 | uint8_t channels; 170 | vector data; 171 | string pixelFormat; 172 | }; 173 | 174 | /// Struct for materials 175 | struct Material 176 | { 177 | optional textureIndex; 178 | Color color; 179 | vector faceIndices; 180 | }; 181 | 182 | } // namespace rviz_mesh_tools_plugins 183 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/kernels/cast_rays.cl: -------------------------------------------------------------------------------- 1 | //#pragma OPENCL EXTENSION cl_intel_printf : enable 2 | 3 | #define EPSILON 0.0000001 4 | #define PI 3.14159265 5 | 6 | typedef struct { 7 | bool intersects; 8 | float intersection; 9 | } intersection_result; 10 | 11 | typedef struct { 12 | float3 vertex0; 13 | float3 vertex1; 14 | float3 vertex2; 15 | } triangle_t; 16 | 17 | /** 18 | * Credits go to: https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm 19 | */ 20 | intersection_result ray_intersects_triangle( 21 | float3 ray, 22 | float3 ray_origin, 23 | triangle_t triangle 24 | ) 25 | { 26 | float3 edge1, edge2, h, s, q; 27 | float a, f, u, v; 28 | intersection_result out; 29 | 30 | edge1 = triangle.vertex1 - triangle.vertex0; 31 | edge2 = triangle.vertex2 - triangle.vertex0; 32 | h = cross(ray, edge2); 33 | a = dot(edge1, h); 34 | 35 | if (a > -EPSILON && a < EPSILON) 36 | { 37 | out.intersects = false; 38 | return out; 39 | } 40 | 41 | f = 1/a; 42 | s = ray_origin - triangle.vertex0; 43 | u = f * dot(s, h); 44 | 45 | if (u < 0.0 || u > 1.0) 46 | { 47 | out.intersects = false; 48 | return out; 49 | } 50 | 51 | q = cross(s, edge1); 52 | v = f * dot(ray, q); 53 | 54 | if (v < 0.0 || u + v > 1.0) 55 | { 56 | out.intersects = false; 57 | return out; 58 | } 59 | 60 | float t = f * dot(edge2, q); 61 | if (t > EPSILON) 62 | { 63 | out.intersects = true; 64 | out.intersection = t; 65 | return out; 66 | } 67 | 68 | out.intersects = false; 69 | return out; 70 | } 71 | 72 | 73 | __kernel void cast_rays( 74 | __global float* vertices, 75 | __global float* ray, 76 | __global float* result 77 | ) 78 | { 79 | int id = get_global_id(0); 80 | 81 | float3 rayOrigin = (float3)(ray[0], ray[1], ray[2]); 82 | float3 rayDirection = (float3)(ray[3], ray[4], ray[5]); 83 | 84 | // initialize result memory 85 | result[id] = -1; 86 | 87 | triangle_t triangle; 88 | triangle.vertex0 = (float3)(vertices[id * 9 + 0], vertices[id * 9 + 1], vertices[id * 9 + 2]); 89 | triangle.vertex1 = (float3)(vertices[id * 9 + 3], vertices[id * 9 + 4], vertices[id * 9 + 5]); 90 | triangle.vertex2 = (float3)(vertices[id * 9 + 6], vertices[id * 9 + 7], vertices[id * 9 + 8]); 91 | 92 | intersection_result i_result = ray_intersects_triangle(rayDirection, rayOrigin, triangle); 93 | if (i_result.intersects) 94 | { 95 | result[id] = i_result.intersection; 96 | } 97 | } 98 | 99 | __kernel void cast_sphere( 100 | __global float* vertices, 101 | __global float* sphere, 102 | __global float* result, 103 | float dist 104 | ) 105 | { 106 | int id = get_global_id(0); 107 | 108 | float3 center = (float3)(sphere[0], sphere[1], sphere[2]); 109 | 110 | // initialize result memory 111 | result[id] = -1; 112 | 113 | // store each input vertex in a float4 to simplify access 114 | float3 vertex0 = (float3)(vertices[id * 9 + 0], vertices[id * 9 + 1], vertices[id * 9 + 2]); 115 | float3 vertex1 = (float3)(vertices[id * 9 + 3], vertices[id * 9 + 4], vertices[id * 9 + 5]); 116 | float3 vertex2 = (float3)(vertices[id * 9 + 6], vertices[id * 9 + 7], vertices[id * 9 + 8]); 117 | 118 | // calculate the distance to the center point for each vertex 119 | float dist0 = distance(center, vertex0); 120 | float dist1 = distance(center, vertex1); 121 | float dist2 = distance(center, vertex2); 122 | 123 | // if one of the vertices is closer to the center than the threshold, the triangle is hit 124 | if (dist0 <= dist || dist1 <= dist || dist2 <= dist) 125 | { 126 | result[id] = 1; 127 | } 128 | 129 | } 130 | 131 | __kernel void cast_box( 132 | __global float* vertices, 133 | __global float4* box, 134 | __global float* result 135 | ) 136 | { 137 | int id = get_global_id(0); 138 | 139 | // store each input vertex in a float4 to simplify access 140 | float4 vertex0 = (float4)(vertices[id * 9 + 0], vertices[id * 9 + 1], vertices[id * 9 + 2], 1.0); 141 | float4 vertex1 = (float4)(vertices[id * 9 + 3], vertices[id * 9 + 4], vertices[id * 9 + 5], 1.0); 142 | float4 vertex2 = (float4)(vertices[id * 9 + 6], vertices[id * 9 + 7], vertices[id * 9 + 8], 1.0); 143 | 144 | // check for each vertex if it lies within the volume spanned by the box planes 145 | bool vertexInVolume0 = true; 146 | bool vertexInVolume1 = true; 147 | bool vertexInVolume2 = true; 148 | for (int planeId = 0; planeId < 6; planeId++) 149 | { 150 | float4 plane = box[planeId]; 151 | vertexInVolume0 = vertexInVolume0 && dot(plane, vertex0) > 0; 152 | vertexInVolume1 = vertexInVolume1 && dot(plane, vertex1) > 0; 153 | vertexInVolume2 = vertexInVolume2 && dot(plane, vertex2) > 0; 154 | } 155 | 156 | // if one of the vertices was in the volume, set this triangle's result to hit 157 | if (vertexInVolume0 || vertexInVolume1 || vertexInVolume2) 158 | { 159 | result[id] = 1; 160 | } 161 | else 162 | { 163 | result[id] = -1; 164 | } 165 | 166 | } 167 | 168 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rviz_mesh_tools_plugins 5 | 6 | RViz display types and tools for the mesh_msgs package. 7 | 8 | 2.0.0 9 | Alexander Mock 10 | Matthias Holoch 11 | Sebastian Pütz 12 | Sebastian Pütz 13 | Kristin Schmidt 14 | Jan Philipp Vogtherr 15 | Malte kleine Piening 16 | 17 | BSD-3 18 | https://github.com/uos/mesh_tools/tree/master/rviz_mesh_tools_plugins 19 | 20 | ament_cmake 21 | 22 | qtbase5-dev 23 | rviz_ogre_vendor 24 | 25 | rclcpp 26 | lvr2 27 | rviz_common 28 | rviz_rendering 29 | rviz_assimp_vendor 30 | std_msgs 31 | mesh_msgs 32 | tf2_ros 33 | pluginlib 34 | message_filters 35 | ocl-icd-opencl-dev 36 | opencl-headers 37 | freeimage 38 | 39 | libqt5-core 40 | libqt5-gui 41 | libqt5-widgets 42 | rviz_ogre_vendor 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/plugins_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget allowing creation of cluster labels. 7 | 8 | 9 | 12 | 13 | A tool allowing selection of clusters. 14 | 15 | 16 | 19 | 20 | Displays labeled clusters of a map. (Don't use without Map3D) 21 | 22 | 23 | 26 | 27 | Displays a map with labeled clusters. 28 | 29 | 30 | 33 | 34 | Displays a mesh. 35 | 36 | 37 | 40 | 41 | Select a goal on a mesh. 42 | 43 | 44 | 47 | 48 | Select a pose estimate on a mesh. 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Robot Operating System code by the University of Osnabrück 4 | # Copyright (c) 2023, University of Osnabrück 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above 12 | # copyright notice, this list of conditions and the following 13 | # disclaimer. 14 | # 15 | # 2. Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # 20 | # 3. Neither the name of the copyright holder nor the names of its 21 | # contributors may be used to endorse or promote products derived 22 | # from this software without specific prior written permission. 23 | # 24 | # 25 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 26 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 27 | # TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 28 | # PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 29 | # CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 30 | # EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 31 | # PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 32 | # OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 33 | # WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 34 | # OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 35 | # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 36 | # 37 | # 38 | # 39 | # rviz_mesh_tools_plugins-extras.cmake 40 | # 41 | # 42 | # authors: 43 | # 44 | # Alexander Mock 45 | # 46 | find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) 47 | find_package(Boost REQUIRED QUITE COMPONENTS system filesystem) -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelDisplay.cpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | */ 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | #include "rclcpp/rclcpp.hpp" 61 | 62 | namespace rviz_mesh_tools_plugins 63 | { 64 | Ogre::ColourValue getRainbowColor(float value) 65 | { 66 | float r = 0.0f; 67 | float g = 0.0f; 68 | float b = 0.0f; 69 | 70 | value = std::min(value, 1.0f); 71 | value = std::max(value, 0.0f); 72 | 73 | float h = value * 5.0f + 1.0f; 74 | int i = floor(h); 75 | float f = h - i; 76 | if (!(i & 1)) 77 | f = 1 - f; // if i is even 78 | float n = 1 - f; 79 | 80 | if (i <= 1) 81 | r = n, g = 0, b = 1; 82 | else if (i == 2) 83 | r = 0, g = n, b = 1; 84 | else if (i == 3) 85 | r = 0, g = 1, b = n; 86 | else if (i == 4) 87 | r = n, g = 1, b = 0; 88 | else if (i >= 5) 89 | r = 1, g = n, b = 0; 90 | 91 | return Ogre::ColourValue(r, g, b, 1.0f); 92 | } 93 | 94 | ClusterLabelDisplay::ClusterLabelDisplay() 95 | : m_activeVisualProperty(nullptr) 96 | , m_alphaProperty(nullptr) 97 | , m_colorsProperty(nullptr) 98 | , m_sphereSizeProperty(nullptr) 99 | , m_phantomVisualProperty(nullptr) 100 | , m_cullingMode(Ogre::CullingMode::CULL_NONE) 101 | { 102 | m_activeVisualProperty = 103 | new rviz_common::properties::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", 104 | this, SLOT(changeVisual()), this); 105 | m_alphaProperty = new rviz_common::properties::FloatProperty("Transparency", 1.0f, 106 | "Transparency of the Labeled Cluster Visualization. 0.0 is fully " 107 | "transparent, 1.0 fully opaque", 108 | this, SLOT(updateColors()), this); 109 | m_alphaProperty->setMin(0.0f); 110 | m_alphaProperty->setMax(1.0f); 111 | 112 | m_colorsProperty = new rviz_common::properties::Property("Colors", "", "colors", this, SLOT(updateColors()), this); 113 | m_colorsProperty->setReadOnly(true); 114 | m_sphereSizeProperty = 115 | new rviz_common::properties::FloatProperty("Brush Size", 50.0f, "Brush Size", this, SLOT(updateSphereSize()), this); 116 | m_phantomVisualProperty = new rviz_common::properties::BoolProperty("Show Phantom", false, 117 | "Show a transparent silhouette of the whole mesh to help with " 118 | "labeling", 119 | this, SLOT(updatePhantomVisual()), this); 120 | 121 | setStatus(rviz_common::properties::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); 122 | } 123 | 124 | ClusterLabelDisplay::~ClusterLabelDisplay() 125 | { 126 | } 127 | 128 | // ===================================================================================================================== 129 | // Public Q_SLOTS 130 | 131 | std::shared_ptr ClusterLabelDisplay::getGeometry() 132 | { 133 | if (!m_geometry) 134 | { 135 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Geometry requested, but none available!"); 136 | } 137 | return m_geometry; 138 | } 139 | 140 | void ClusterLabelDisplay::setData(shared_ptr geometry, vector clusters) 141 | { 142 | if (has_data) 143 | { 144 | RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: already has data, but setData() was called again!"); 145 | } 146 | 147 | // Copy data 148 | m_geometry = geometry; 149 | m_clusterList = clusters; 150 | m_clusterList.insert(m_clusterList.begin(), Cluster("__NEW__", vector())); 151 | 152 | // Set flag 153 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: received data"); 154 | has_data = true; 155 | 156 | // Draw visuals 157 | if(isEnabled()) 158 | { 159 | updateMap(); 160 | } 161 | 162 | setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); 163 | } 164 | 165 | void ClusterLabelDisplay::setCullingMode(Ogre::CullingMode mode) 166 | { 167 | auto* tool = getOrCreateLabelTool(); 168 | if (tool) 169 | { 170 | tool->setCullingMode(mode); 171 | } 172 | for (auto& visual: m_visuals) 173 | { 174 | visual->setCullingMode(mode); 175 | } 176 | } 177 | 178 | // ===================================================================================================================== 179 | // Callbacks 180 | 181 | void ClusterLabelDisplay::onInitialize() 182 | { 183 | // Look for an existing label tool or create a new one 184 | ClusterLabelTool* tool = getOrCreateLabelTool(); 185 | 186 | // Set the Tool related configuration 187 | tool->setBrushSize(m_sphereSizeProperty->getFloat()); 188 | } 189 | 190 | void ClusterLabelDisplay::onEnable() 191 | { 192 | updateMap(); 193 | } 194 | 195 | void ClusterLabelDisplay::onDisable() 196 | { 197 | m_visuals.clear(); 198 | m_phantomVisual.reset(); 199 | 200 | if (auto* tool = getOrCreateLabelTool()) 201 | { 202 | tool->resetVisual(); 203 | } 204 | } 205 | 206 | // ===================================================================================================================== 207 | // Callbacks triggered from UI events (mostly) 208 | 209 | void ClusterLabelDisplay::changeVisual() 210 | { 211 | if (m_activeVisualProperty->getStdString().empty()) 212 | { 213 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Should change visual but no visual selected!"); 214 | return; 215 | } 216 | 217 | RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); 218 | 219 | m_activeVisualId = m_activeVisualProperty->getOptionInt(); 220 | 221 | // Active visual has changed, notify label tool that it has to refresh its pointer on the active visual 222 | notifyLabelTool(); 223 | } 224 | 225 | void ClusterLabelDisplay::updateMap() 226 | { 227 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Update"); 228 | 229 | if(!has_data) 230 | { 231 | RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: No data available! Can't show map"); 232 | return; 233 | } 234 | 235 | auto* tool = getOrCreateLabelTool(); 236 | 237 | if(tool) 238 | { 239 | // Reset the visual of the label tool so that it can be deleted 240 | tool->resetVisual(); 241 | } else { 242 | setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "ClusterLabel Tool could not be initialized! It will not work!"); 243 | return; 244 | } 245 | 246 | // Now create the visuals for the loaded clusters 247 | createVisualsFromClusterList(); 248 | 249 | // Fill options for dropdown menu containing the cluster names 250 | fillPropertyOptions(); 251 | 252 | // Create a phantom visual if it is enabled 253 | updatePhantomVisual(); 254 | 255 | // Notify label tool for changes. The label tool should now destroy its visual and get a new one from this obj 256 | notifyLabelTool(); 257 | 258 | // Apply the default colors to the visuals 259 | updateColors(); 260 | 261 | // Update the tool's assigned display (to this display) 262 | if (tool) 263 | { 264 | tool->setDisplay(this); 265 | } 266 | 267 | // All good 268 | setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); 269 | } 270 | 271 | void ClusterLabelDisplay::updateColors() 272 | { 273 | for (size_t i = 0; i < m_colorProperties.size(); i++) 274 | { 275 | auto colorProp = m_colorProperties[i]; 276 | m_visuals[i]->setColor(colorProp->getOgreColor(), m_alphaProperty->getFloat()); 277 | } 278 | } 279 | 280 | void ClusterLabelDisplay::updateSphereSize() 281 | { 282 | if (auto* tool = getOrCreateLabelTool()) 283 | { 284 | tool->setBrushSize(m_sphereSizeProperty->getFloat()); 285 | } 286 | } 287 | 288 | void ClusterLabelDisplay::updatePhantomVisual() 289 | { 290 | if (!m_phantomVisualProperty->getBool()) 291 | { 292 | m_phantomVisual.reset(nullptr); 293 | } 294 | else if (!m_phantomVisual) 295 | { 296 | createPhantomVisual(); 297 | } 298 | } 299 | 300 | void ClusterLabelDisplay::fillPropertyOptions() 301 | { 302 | // Clear options 303 | m_activeVisualProperty->clearOptions(); 304 | m_colorsProperty->removeChildren(); 305 | m_colorProperties.clear(); 306 | 307 | for (size_t i = 0; i < m_clusterList.size(); i++) 308 | { 309 | // Add cluster labels to dropdown menu 310 | m_activeVisualProperty->addOption(QString::fromStdString(m_clusterList[i].name), i); 311 | 312 | // Add color options 313 | Ogre::ColourValue rainbowColor = getRainbowColor((((float)i + 1) / m_clusterList.size())); 314 | m_colorProperties.emplace_back(new rviz_common::properties::ColorProperty( 315 | QString::fromStdString(m_clusterList[i].name), 316 | QColor(rainbowColor.r * 255, rainbowColor.g * 255, rainbowColor.b * 255), 317 | QString::fromStdString(m_clusterList[i].name), m_colorsProperty, SLOT(updateColors()), this)); 318 | } 319 | } 320 | 321 | // ===================================================================================================================== 322 | // Visuals logic 323 | 324 | void ClusterLabelDisplay::createVisualsFromClusterList() 325 | { 326 | // Destroy all current visuals 327 | if (!m_visuals.empty()) 328 | { 329 | m_visuals.clear(); 330 | } 331 | 332 | // Create a visual for each entry in the cluster list 333 | float colorIndex = 0.0; // index for coloring 334 | for (size_t i = 0; i < m_clusterList.size(); i++) 335 | { 336 | std::stringstream ss; 337 | ss << "ClusterLabelVisual_" << i; 338 | 339 | auto visual = std::make_shared(context_, ss.str(), m_geometry); 340 | RCLCPP_DEBUG( 341 | rclcpp::get_logger("rviz_mesh_tools_plugins"), 342 | "Label Display: Create visual for label '%s' with %lu faces", m_clusterList[i].name.c_str(), m_clusterList[i].faces.size() 343 | ); 344 | visual->setFacesInCluster(m_clusterList[i].faces); 345 | visual->setColor(getRainbowColor((++colorIndex / m_clusterList.size())), m_alphaProperty->getFloat()); 346 | visual->setCullingMode(m_cullingMode); 347 | m_visuals.push_back(visual); 348 | } 349 | } 350 | 351 | void ClusterLabelDisplay::createPhantomVisual() 352 | { 353 | m_phantomVisual.reset(new ClusterLabelVisual(context_, "ClusterLabelPhantomVisual", m_geometry)); 354 | vector allFacesVector; 355 | for (uint32_t i = 0; i < m_geometry->faces.size(); i++) 356 | { 357 | allFacesVector.push_back(i); 358 | } 359 | m_phantomVisual->setFacesInCluster(allFacesVector); 360 | m_phantomVisual->setColor(Ogre::ColourValue(0.2, 0.3, 0.2), 0.1); 361 | } 362 | 363 | // ===================================================================================================================== 364 | // Label tool 365 | 366 | ClusterLabelTool* ClusterLabelDisplay::getOrCreateLabelTool() 367 | { 368 | ClusterLabelTool* tool = nullptr; 369 | 370 | // Check if the cluster label tool is already opened 371 | rviz_common::ToolManager* toolManager = context_->getToolManager(); 372 | QStringList toolClasses = toolManager->getToolClasses(); 373 | for (int i = 0; i < toolClasses.size(); i++) 374 | { 375 | if (toolClasses[i].contains("ClusterLabel")) 376 | { 377 | tool = dynamic_cast(toolManager->getTool(i)); 378 | if (nullptr == tool) 379 | { 380 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found Tool with 'ClusterLabel' class but could not dynamic_cast to ClusterLabelTool*"); 381 | return nullptr; 382 | } 383 | } 384 | } 385 | 386 | // Create a Tool instance 387 | if (!tool) 388 | { 389 | auto tool_tmp = context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel"); 390 | if(tool = dynamic_cast(tool_tmp); tool != nullptr) 391 | { 392 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Created ClusterLabelTool at Address: %llx", (unsigned long long) tool); 393 | } 394 | else 395 | { 396 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not create ClusterLabelTool"); 397 | return nullptr; 398 | } 399 | } 400 | 401 | return tool; 402 | } 403 | 404 | void ClusterLabelDisplay::notifyLabelTool() 405 | { 406 | auto* tool = getOrCreateLabelTool(); 407 | if (!tool) 408 | { 409 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed to notify LabelTool of active Visual Change!"); 410 | return; 411 | } 412 | tool->setVisual(m_visuals[m_activeVisualId]); 413 | } 414 | 415 | void ClusterLabelDisplay::addLabel(std::string label, std::vector faces) 416 | { 417 | RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Display: add label '" << label << "'"); 418 | 419 | Q_EMIT signalAddLabel(Cluster(label, faces)); 420 | } 421 | 422 | } // End namespace rviz_mesh_tools_plugins 423 | 424 | #include 425 | PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelDisplay, rviz_common::Display) 426 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelPanel.cpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | */ 48 | 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | #include 60 | 61 | namespace rviz_mesh_tools_plugins 62 | { 63 | ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) 64 | :rviz_common::Panel(parent) 65 | { 66 | QHBoxLayout* clusterNameLayout = new QHBoxLayout(); 67 | clusterNameLayout->addWidget(new QLabel("Cluster Name:")); 68 | m_clusterNameEditor = new QLineEdit(); 69 | clusterNameLayout->addWidget(m_clusterNameEditor); 70 | 71 | m_createClusterButton = new QPushButton("Label Cluster"); 72 | 73 | m_resetFacesButton = new QPushButton("Reset Faces"); 74 | 75 | QVBoxLayout* layout = new QVBoxLayout(); 76 | layout->addLayout(clusterNameLayout); 77 | layout->addWidget(m_createClusterButton); 78 | layout->addWidget(m_resetFacesButton); 79 | setLayout(layout); 80 | 81 | // Make signal/slot connections 82 | connect(m_clusterNameEditor, SIGNAL(editingFinished()), this, SLOT(updateClusterName())); 83 | 84 | connect(m_createClusterButton, SIGNAL(released()), this, SLOT(publish())); 85 | connect(m_resetFacesButton, SIGNAL(released()), this, SLOT(resetFaces())); 86 | } 87 | 88 | void ClusterLabelPanel::onInitialize() 89 | { 90 | // Check if the cluster label tool is already opened 91 | 92 | rviz_common::ToolManager* toolManager = this->getDisplayContext()->getToolManager(); 93 | QStringList toolClasses = toolManager->getToolClasses(); 94 | bool foundTool = false; 95 | for (int i = 0; i < toolClasses.size(); i++) 96 | { 97 | if (toolClasses.at(i).contains("ClusterLabel")) 98 | { 99 | m_tool = static_cast(toolManager->getTool(i)); 100 | foundTool = true; 101 | break; 102 | } 103 | } 104 | 105 | if (!foundTool) 106 | { 107 | m_tool = static_cast(this->getDisplayContext()->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); 108 | } 109 | } 110 | 111 | void ClusterLabelPanel::setClusterName(const QString& clusterName) 112 | { 113 | m_clusterName = clusterName; 114 | Q_EMIT configChanged(); 115 | 116 | // Gray out the create cluster button when the cluster name is empty 117 | m_createClusterButton->setEnabled(m_clusterName != ""); 118 | } 119 | 120 | void ClusterLabelPanel::updateClusterName() 121 | { 122 | setClusterName(m_clusterNameEditor->text()); 123 | } 124 | 125 | void ClusterLabelPanel::publish() 126 | { 127 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Panel: Publish"); 128 | m_tool->publishLabel(m_clusterName.toStdString()); 129 | } 130 | 131 | void ClusterLabelPanel::resetFaces() 132 | { 133 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label panel: Reset"); 134 | m_tool->resetFaces(); 135 | } 136 | 137 | void ClusterLabelPanel::save(rviz_common::Config config) const 138 | { 139 | rviz_common::Panel::save(config); 140 | config.mapSetValue("ClusterName", m_clusterName); 141 | } 142 | 143 | void ClusterLabelPanel::load(const rviz_common::Config& config) 144 | { 145 | rviz_common::Panel::load(config); 146 | QString clusterName; 147 | if (config.mapGetString("ClusterName", &clusterName)) 148 | { 149 | m_clusterNameEditor->setText(clusterName); 150 | updateClusterName(); 151 | } 152 | } 153 | 154 | } // End namespace rviz_mesh_tools_plugins 155 | 156 | #include 157 | PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelPanel, rviz_common::Panel) 158 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * ClusterLabelVisual.cpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | */ 47 | 48 | #include 49 | 50 | #include 51 | 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | namespace rviz_mesh_tools_plugins 62 | { 63 | // ClusterLabelVisual::ClusterLabelVisual( 64 | // rviz_common::DisplayContext* context, 65 | // std::string labelId) 66 | // :m_displayContext(context) 67 | // ,m_labelId(labelId) 68 | // { 69 | // } 70 | 71 | ClusterLabelVisual::ClusterLabelVisual( 72 | rviz_common::DisplayContext* context, 73 | std::string labelId, 74 | std::shared_ptr geometry) 75 | :m_displayContext(context) 76 | ,m_labelId(labelId) 77 | ,m_geometry(geometry) 78 | { 79 | 80 | // Get or create scene node 81 | Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); 82 | Ogre::SceneNode* rootNode = sceneManager->getRootSceneNode(); 83 | 84 | std::stringstream strstream; 85 | strstream << "ClusterLabelScene"; 86 | std::string sceneId = strstream.str(); 87 | 88 | if (sceneManager->hasSceneNode(sceneId)) 89 | { 90 | m_sceneNode = (Ogre::SceneNode*)(rootNode->getChild(sceneId)); 91 | } 92 | else 93 | { 94 | m_sceneNode = rootNode->createChildSceneNode(sceneId); 95 | } 96 | 97 | // Retrieve or create the mesh and attach it to the scene node 98 | m_mesh = Ogre::MeshManager::getSingleton().getByName("ClusterLabelMesh", "General"); 99 | if(m_mesh.isNull() && geometry) 100 | { 101 | m_mesh = Ogre::MeshManager::getSingleton().createManual("ClusterLabelMesh", "General"); 102 | 103 | // Create the vertex data structure 104 | m_mesh->sharedVertexData = new Ogre::VertexData; 105 | m_mesh->sharedVertexData->vertexCount = geometry->vertices.size(); 106 | 107 | // Declare how the vertices will be represented 108 | Ogre::VertexDeclaration* decl = m_mesh->sharedVertexData->vertexDeclaration; 109 | size_t offset = 0; 110 | 111 | // The first three floats of each vertex represent the position 112 | decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION); 113 | offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); 114 | 115 | // Create the vertex buffer 116 | Ogre::HardwareVertexBufferSharedPtr vertexBuffer = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( 117 | offset, m_mesh->sharedVertexData->vertexCount, Ogre::HardwareBuffer::HBU_STATIC); 118 | 119 | // Lock the buffer so we can get exclusive access to its data 120 | float* vertices = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_NORMAL)); 121 | 122 | // Write the mesh data into the buffer 123 | for (int i = 0; i < m_mesh->sharedVertexData->vertexCount; i++) 124 | { 125 | vertices[(i * 3) + 0] = geometry->vertices[i].x; 126 | vertices[(i * 3) + 1] = geometry->vertices[i].y; 127 | vertices[(i * 3) + 2] = geometry->vertices[i].z; 128 | } 129 | 130 | // Unlock the buffer 131 | vertexBuffer->unlock(); 132 | 133 | // Attach the vertex buffer to the mesh 134 | m_mesh->sharedVertexData->vertexBufferBinding->setBinding(0, vertexBuffer); 135 | 136 | // Set the bounds of the mesh 137 | // TODO: Calculate the correct bounding box 138 | m_mesh->_setBounds(Ogre::AxisAlignedBox::EXTENT_INFINITE); 139 | 140 | // Notify the mesh that we're all ready 141 | m_mesh->load(); 142 | 143 | // Create an entity of the mesh and add it to the scene 144 | Ogre::Entity* entity = sceneManager->createEntity("ClusterLabelEntity", "ClusterLabelMesh", "General"); 145 | if(!entity) 146 | { 147 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "nullptr return: sceneManager->createEntity(\"ClusterLabelEntity\", \"ClusterLabelMesh\", \"General\"); "); 148 | } 149 | entity->setMaterialName("CustomMaterial", "General"); 150 | entity->setVisibilityFlags(m_displayContext->getDefaultVisibilityBit()); 151 | m_sceneNode->attachObject(entity); 152 | } 153 | 154 | // Create a submesh and a custom material for it 155 | if(!m_mesh.isNull()) 156 | { 157 | m_subMesh = m_mesh->createSubMesh(m_labelId); 158 | m_subMesh->useSharedVertices = true; 159 | std::stringstream sstm; 160 | sstm << "ClusterLabel_Material_" << m_labelId; 161 | 162 | m_material = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 163 | if(m_material.isNull()) 164 | { 165 | m_material = Ogre::MaterialManager::getSingleton().create( 166 | sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); 167 | } 168 | 169 | m_subMesh->setMaterialName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 170 | m_material->getTechnique(0)->removeAllPasses(); 171 | m_material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 172 | m_material->setSceneBlending(Ogre::SBT_ADD); 173 | m_material->setDepthWriteEnabled(false); 174 | 175 | initMaterial(); 176 | } 177 | } 178 | 179 | ClusterLabelVisual::~ClusterLabelVisual() 180 | { 181 | reset(); 182 | 183 | if (!m_mesh.isNull()) 184 | { 185 | RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); 186 | 187 | try 188 | { 189 | m_mesh->destroySubMesh(m_labelId); 190 | } 191 | catch (...) 192 | { 193 | RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Exception in Visual destructor"); 194 | } 195 | } 196 | 197 | if (m_sceneNode->numAttachedObjects() == 0) 198 | { 199 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); 200 | m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); 201 | } 202 | 203 | } 204 | 205 | void ClusterLabelVisual::reset() 206 | { 207 | RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Reset ClusterLabelVisual"); 208 | if(!m_material.isNull()) 209 | { 210 | // if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) 211 | // { 212 | m_material->unload(); 213 | Ogre::MaterialManager::getSingleton().remove(m_material); 214 | // } else { 215 | // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << m_material->getName() << "' to unload. skipping"); 216 | // } 217 | } else { 218 | 219 | } 220 | } 221 | 222 | void ClusterLabelVisual::setGeometry(std::shared_ptr geometry) 223 | { 224 | } 225 | 226 | void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) 227 | { 228 | m_faces = faces; 229 | 230 | if(!m_geometry) 231 | { 232 | RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); 233 | return; 234 | } 235 | 236 | // don't draw the cluster if there are no faces in it 237 | if (faces.empty()) 238 | { 239 | m_subMesh->indexData->indexBuffer.setNull(); 240 | m_subMesh->indexData->indexCount = 0; 241 | m_subMesh->indexData->indexStart = 0; 242 | m_material->getTechnique(0)->removeAllPasses(); 243 | RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: faces empty!"); 244 | return; 245 | } 246 | 247 | // if there are faces and there are no passes, create a pass to draw the cluster 248 | if (m_material->getTechnique(0)->getNumPasses() == 0) 249 | { 250 | m_material->getTechnique(0)->createPass(); 251 | m_material->setDiffuse(m_color); 252 | m_material->setSelfIllumination(m_color); 253 | initMaterial(); 254 | } 255 | 256 | size_t indexCount = faces.size() * 3; 257 | 258 | // Create the index buffer 259 | Ogre::HardwareIndexBufferSharedPtr indexBuffer = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer( 260 | Ogre::HardwareIndexBuffer::IT_32BIT, indexCount, Ogre::HardwareBuffer::HBU_WRITE_ONLY); 261 | 262 | // Lock the buffer so we can get exclusive access to its data 263 | uint32_t* indices = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_WRITE_ONLY)); 264 | 265 | // Define the triangles 266 | for (int i = 0; i < faces.size(); i++) 267 | { 268 | uint32_t faceId = faces[i]; 269 | indices[i * 3 + 0] = m_geometry->faces[faceId].vertexIndices[0]; 270 | indices[i * 3 + 1] = m_geometry->faces[faceId].vertexIndices[1]; 271 | indices[i * 3 + 2] = m_geometry->faces[faceId].vertexIndices[2]; 272 | } 273 | 274 | // Unlock the buffer 275 | indexBuffer->unlock(); 276 | 277 | // Attach the index buffer to the submesh 278 | m_subMesh->indexData->indexBuffer = indexBuffer; 279 | m_subMesh->indexData->indexCount = indexCount; 280 | m_subMesh->indexData->indexStart = 0; 281 | } 282 | 283 | void ClusterLabelVisual::setColor(Ogre::ColourValue facesColor, float alpha) 284 | { 285 | if (!m_material.isNull()) 286 | { 287 | facesColor.a = alpha; 288 | m_material->setDiffuse(facesColor); 289 | m_material->setSelfIllumination(facesColor); 290 | m_color = facesColor; 291 | } 292 | } 293 | 294 | void ClusterLabelVisual::setCullingMode(Ogre::CullingMode mode) 295 | { 296 | if (m_material) 297 | { 298 | m_material->setCullingMode(mode); 299 | } 300 | } 301 | 302 | void ClusterLabelVisual::initMaterial() 303 | { 304 | m_material->setCullingMode(Ogre::CULL_NONE); 305 | m_material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 306 | m_material->setDepthWriteEnabled(false); 307 | } 308 | 309 | } // End namespace rviz_mesh_tools_plugins 310 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/InteractionHelper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rviz_mesh_tools_plugins 13 | { 14 | 15 | Ogre::Ray getMouseEventRay( 16 | const rviz_common::ViewportMouseEvent& event) 17 | { 18 | auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()); 19 | int width = viewport->getActualWidth(); 20 | int height = viewport->getActualHeight(); 21 | Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( 22 | static_cast(event.x) / static_cast(width), 23 | static_cast(event.y) / static_cast(height)); 24 | return mouse_ray; 25 | } 26 | 27 | 28 | void getRawManualObjectData( 29 | const Ogre::Affine3& to_world, 30 | Ogre::ManualObject::ManualObjectSection& section, 31 | size_t& vertexCount, Ogre::Vector3*& vertices, 32 | size_t& indexCount, unsigned long*& indices) 33 | { 34 | Ogre::VertexData* vertexData; 35 | const Ogre::VertexElement* vertexElement; 36 | Ogre::HardwareVertexBufferSharedPtr vertexBuffer; 37 | unsigned char* vertexChar; 38 | float* vertexFloat; 39 | 40 | vertexData = section.getRenderOperation()->vertexData; 41 | vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION); 42 | vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource()); 43 | vertexChar = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); 44 | 45 | vertexCount = vertexData->vertexCount; 46 | vertices = new Ogre::Vector3[vertexCount]; 47 | 48 | for (size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize()) 49 | { 50 | vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat); 51 | vertices[i] = to_world * Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]); 52 | } 53 | 54 | vertexBuffer->unlock(); 55 | 56 | Ogre::IndexData* indexData; 57 | Ogre::HardwareIndexBufferSharedPtr indexBuffer; 58 | indexData = section.getRenderOperation()->indexData; 59 | indexCount = indexData->indexCount; 60 | indices = new unsigned long[indexCount]; 61 | indexBuffer = indexData->indexBuffer; 62 | unsigned int* pLong = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); 63 | unsigned short* pShort = reinterpret_cast(pLong); 64 | 65 | for (size_t i = 0; i < indexCount; i++) 66 | { 67 | unsigned long index; 68 | if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT) 69 | { 70 | index = static_cast(pLong[i]); 71 | } 72 | else 73 | { 74 | index = static_cast(pShort[i]); 75 | } 76 | 77 | indices[i] = index; 78 | } 79 | indexBuffer->unlock(); 80 | } 81 | 82 | bool selectFace( 83 | const Ogre::ManualObject* mesh, 84 | const Ogre::Ray& ray, 85 | Intersection& intersection) 86 | { 87 | Ogre::Real dist = -1.0f; 88 | Ogre::Vector3 a, b, c; 89 | 90 | size_t vertex_count = 0; 91 | Ogre::Vector3* vertices; 92 | size_t index_count = 0; 93 | unsigned long* indices; 94 | Ogre::Affine3 mesh_to_world = mesh->_getParentNodeFullTransform(); 95 | 96 | /* ManualObject::getNumSections and ManualObject::getSection are deprecated 97 | * in the Ogre version ROS Jazzy (and future versions) uses. Since the new 98 | * API is not available in the Ogre version used by ROS Humble we use this 99 | * check to keep Humble support. 100 | * 101 | * This can be removed when Humble support is dropped. 102 | */ 103 | #if OGRE_VERSION < ((1 << 16) | (12 << 8) | 7) 104 | for (size_t i = 0; i < mesh->getNumSections(); i++) 105 | { 106 | const auto& section = mesh->getSection(i); 107 | #else 108 | for (const auto& section: mesh->getSections()) 109 | { 110 | #endif 111 | getRawManualObjectData(mesh_to_world, *section, vertex_count, vertices, index_count, indices); 112 | 113 | // All sections and materials should have the same culling mode, so we use the first we find 114 | std::optional culling_mode; 115 | for (auto technique: section->getMaterial()->getTechniques()) 116 | { 117 | for (auto pass: technique->getPasses()) 118 | { 119 | culling_mode = pass->getCullingMode(); 120 | break; 121 | } 122 | if (culling_mode) 123 | { 124 | break; 125 | } 126 | } 127 | 128 | if (!culling_mode) 129 | { 130 | culling_mode = Ogre::CullingMode::CULL_NONE; 131 | } 132 | 133 | bool int_pos = true; 134 | bool int_neg = true; 135 | switch(culling_mode.value()) 136 | { 137 | case Ogre::CULL_CLOCKWISE: 138 | int_pos = true; 139 | int_neg = false; 140 | break; 141 | case Ogre::CULL_ANTICLOCKWISE: 142 | int_pos = false; 143 | int_neg = true; 144 | break; 145 | case Ogre::CULL_NONE: 146 | int_pos = true; 147 | int_neg = true; 148 | } 149 | 150 | if (index_count != 0) 151 | { 152 | for (size_t face_id = 0; face_id < index_count / 3; face_id++) 153 | { 154 | // face: indices[j], 155 | const Ogre::Vector3 vertex; 156 | 157 | std::pair goal = Ogre::Math::intersects(ray, 158 | vertices[indices[face_id * 3 + 0]], 159 | vertices[indices[face_id * 3 + 1]], 160 | vertices[indices[face_id * 3 + 2]], int_pos, int_neg); 161 | 162 | if (goal.first) 163 | { 164 | if ((dist < 0.0f) || (goal.second < dist)) 165 | { 166 | dist = goal.second; 167 | a = vertices[indices[face_id * 3 + 0]]; 168 | b = vertices[indices[face_id * 3 + 1]]; 169 | c = vertices[indices[face_id * 3 + 2]]; 170 | intersection.face_id = face_id; 171 | } 172 | } 173 | } 174 | } 175 | } 176 | 177 | delete[] vertices; 178 | delete[] indices; 179 | if (dist != -1) 180 | { 181 | intersection.range = dist; 182 | intersection.point = ray.getPoint(dist); 183 | Ogre::Vector3 ab = b - a; 184 | Ogre::Vector3 ac = c - a; 185 | intersection.normal = ac.crossProduct(ab).normalisedCopy(); 186 | return true; 187 | } 188 | else 189 | { 190 | return false; 191 | } 192 | } 193 | 194 | bool selectFace( 195 | Ogre::SceneManager* sm, 196 | const Ogre::Ray& ray, 197 | Intersection& intersection) 198 | { 199 | Ogre::RaySceneQuery* query = sm->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); 200 | query->setSortByDistance(true); 201 | 202 | Ogre::RaySceneQueryResult& result = query->execute(); 203 | 204 | for (size_t i = 0; i < result.size(); i++) 205 | { 206 | if (result[i].movable->getName().find("TriangleMesh") != std::string::npos) 207 | { 208 | Ogre::ManualObject* mesh = static_cast(result[i].movable); 209 | if (selectFace(mesh, ray, intersection)) 210 | { 211 | return true; 212 | } 213 | } 214 | } 215 | return false; 216 | } 217 | 218 | bool selectFace( 219 | const rviz_common::DisplayContext* ctx, 220 | const Ogre::Ray& ray, 221 | Intersection& intersection) 222 | { 223 | return selectFace(ctx->getSceneManager(), ray, intersection); 224 | } 225 | 226 | } // namespace rviz_mesh_tools_plugins 227 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/MeshGoalTool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshGoalTool.cpp 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | 46 | #include "rviz_mesh_tools_plugins/MeshGoalTool.hpp" 47 | 48 | #include 49 | PLUGINLIB_EXPORT_CLASS( rviz_mesh_tools_plugins::MeshGoalTool, rviz_common::Tool ) 50 | 51 | namespace rviz_mesh_tools_plugins 52 | { 53 | 54 | MeshGoalTool::MeshGoalTool() 55 | { 56 | shortcut_key_ = 'm'; 57 | topic_property_ = new rviz_common::properties::StringProperty( "Topic", "goal", 58 | "The topic on which to publish the mesh navigation goals.", 59 | getPropertyContainer(), SLOT(updateTopic()), this); 60 | 61 | switch_bottom_top_ = new rviz_common::properties::BoolProperty("Switch Bottom/Top", 62 | false, "Enable to stwich the bottom and top.", 63 | getPropertyContainer()); 64 | 65 | } 66 | 67 | void MeshGoalTool::onInitialize() 68 | { 69 | MeshPoseTool::onInitialize(); 70 | setName( "Mesh Goal" ); 71 | updateTopic(); 72 | } 73 | 74 | void MeshGoalTool::updateTopic() 75 | { 76 | auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); 77 | pose_pub_ = node->create_publisher(topic_property_->getStdString(), 1); 78 | } 79 | 80 | void MeshGoalTool::onPoseSet( 81 | const Ogre::Vector3& position, 82 | const Ogre::Quaternion& orientation ) 83 | { 84 | geometry_msgs::msg::PoseStamped msg; 85 | msg.pose.position.x = position.x; 86 | msg.pose.position.y = position.y; 87 | msg.pose.position.z = position.z; 88 | 89 | // ogreToRos(x,y,z) = (-z,-x,y) 90 | 91 | Ogre::Quaternion ros_orientation; 92 | 93 | if(switch_bottom_top_->getBool()) 94 | { 95 | ros_orientation.FromAxes( 96 | -orientation.zAxis(), 97 | orientation.xAxis(), 98 | -orientation.yAxis() 99 | ); 100 | } 101 | else 102 | { 103 | ros_orientation.FromAxes( 104 | -orientation.zAxis(), 105 | -orientation.xAxis(), 106 | orientation.yAxis() 107 | ); 108 | } 109 | 110 | msg.pose.orientation.x = ros_orientation.x; 111 | msg.pose.orientation.y = ros_orientation.y; 112 | msg.pose.orientation.z = ros_orientation.z; 113 | msg.pose.orientation.w = ros_orientation.w; 114 | 115 | auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); 116 | msg.header.stamp = node->now(); 117 | msg.header.frame_id = context_->getFixedFrame().toStdString(); 118 | pose_pub_->publish(msg); 119 | } 120 | 121 | } // namespace rviz_mesh_tools_plugins 122 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/MeshPoseGuessTool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2023, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshGoalTool.cpp 41 | * 42 | * author: Alexander Mock 43 | */ 44 | 45 | 46 | #include "rviz_mesh_tools_plugins/MeshPoseGuessTool.hpp" 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | 54 | #include 55 | PLUGINLIB_EXPORT_CLASS( rviz_mesh_tools_plugins::MeshPoseGuessTool, rviz_common::Tool ) 56 | 57 | namespace rviz_mesh_tools_plugins 58 | { 59 | 60 | MeshPoseGuessTool::MeshPoseGuessTool() 61 | :qos_profile_(5) 62 | { 63 | shortcut_key_ = 'e'; 64 | topic_property_ = new rviz_common::properties::StringProperty( "Topic", "initialpose", 65 | "The topic on which to publish the mesh pose estimate.", 66 | getPropertyContainer(), SLOT(updateTopic()), this); 67 | qos_profile_property_ = new rviz_common::properties::QosProfileProperty( 68 | topic_property_, qos_profile_); 69 | 70 | switch_bottom_top_ = new rviz_common::properties::BoolProperty("Switch Bottom/Top", 71 | false, "Enable to stwich the bottom and top.", 72 | getPropertyContainer()); 73 | 74 | 75 | cov_position_x_property_ = new rviz_common::properties::FloatProperty("Covariance position x", 0.5f * 0.5f, "Covariance ppositionos x", getPropertyContainer()); 76 | cov_position_y_property_ = new rviz_common::properties::FloatProperty("Covariance position y", 0.5f * 0.5f, "Covariance position y", getPropertyContainer()); 77 | cov_position_z_property_ = new rviz_common::properties::FloatProperty("Covariance position z", 0.1f * 0.1f, "Covariance position z", getPropertyContainer()); 78 | cov_orientation_x_property_ = new rviz_common::properties::FloatProperty("Covariance orientation x", 0.1f * 0.1f, "Covariance orientation x", getPropertyContainer()); 79 | cov_orientation_y_property_ = new rviz_common::properties::FloatProperty("Covariance orientation y", 0.1f * 0.1f, "Covariance orientation y", getPropertyContainer()); 80 | cov_orientation_z_property_ = new rviz_common::properties::FloatProperty("Covariance orientation z", static_cast(M_PI / 12.0 * M_PI / 12.0), "Covariance orientation z", getPropertyContainer()); 81 | } 82 | 83 | void MeshPoseGuessTool::onInitialize() 84 | { 85 | MeshPoseTool::onInitialize(); 86 | qos_profile_property_->initialize( 87 | [this](rclcpp::QoS profile) {this->qos_profile_ = profile;}); 88 | setName("Mesh Pose Estimate"); 89 | updateTopic(); 90 | 91 | setColor(1.0f, 0.0f, 0.0f, 1.0f); 92 | } 93 | 94 | void MeshPoseGuessTool::updateTopic() 95 | { 96 | auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); 97 | pose_pub_ = node->create_publisher(topic_property_->getStdString(), qos_profile_); 98 | clock_ = node->get_clock(); 99 | } 100 | 101 | void MeshPoseGuessTool::onPoseSet( 102 | const Ogre::Vector3& position, 103 | const Ogre::Quaternion& orientation ) 104 | { 105 | geometry_msgs::msg::PoseWithCovarianceStamped msg; 106 | msg.pose.pose.position.x = position.x; 107 | msg.pose.pose.position.y = position.y; 108 | msg.pose.pose.position.z = position.z; 109 | 110 | // ogreToRos(x,y,z) = (-z,-x,y) 111 | 112 | Ogre::Quaternion ros_orientation; 113 | 114 | if(switch_bottom_top_->getBool()) 115 | { 116 | ros_orientation.FromAxes( 117 | -orientation.zAxis(), 118 | orientation.xAxis(), 119 | -orientation.yAxis() 120 | ); 121 | } 122 | else 123 | { 124 | ros_orientation.FromAxes( 125 | -orientation.zAxis(), 126 | -orientation.xAxis(), 127 | orientation.yAxis() 128 | ); 129 | } 130 | 131 | msg.pose.pose.orientation.x = ros_orientation.x; 132 | msg.pose.pose.orientation.y = ros_orientation.y; 133 | msg.pose.pose.orientation.z = ros_orientation.z; 134 | msg.pose.pose.orientation.w = ros_orientation.w; 135 | 136 | for(size_t i=0; i<36; i++) 137 | { 138 | msg.pose.covariance[i] = 0.0; 139 | } 140 | 141 | msg.pose.covariance[0*6+0] = cov_position_x_property_->getFloat(); 142 | msg.pose.covariance[1*6+1] = cov_position_y_property_->getFloat(); 143 | msg.pose.covariance[2*6+2] = cov_position_z_property_->getFloat(); 144 | msg.pose.covariance[3*6+3] = cov_orientation_x_property_->getFloat(); 145 | msg.pose.covariance[4*6+4] = cov_orientation_y_property_->getFloat(); 146 | msg.pose.covariance[5*6+5] = cov_orientation_z_property_->getFloat(); 147 | 148 | msg.header.stamp = clock_->now(); 149 | msg.header.frame_id = context_->getFixedFrame().toStdString(); 150 | pose_pub_->publish(msg); 151 | } 152 | 153 | } // namespace rviz_mesh_tools_plugins 154 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/MeshPoseTool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2015, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * MeshPoseTool.cpp 41 | * 42 | * authors: Sebastian Pütz 43 | * Alexander Mock 44 | * 45 | */ 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include "rviz_rendering/render_window.hpp" 57 | 58 | // #include "rviz_rendering/render_window.hpp" 59 | 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | // method is private :( 67 | // #include 68 | #include 69 | #include 70 | 71 | #include 72 | 73 | #include "rviz_mesh_tools_plugins/MeshPoseTool.hpp" 74 | 75 | #include 76 | 77 | 78 | namespace rviz_mesh_tools_plugins 79 | { 80 | 81 | MeshPoseTool::MeshPoseTool() 82 | :rviz_common::Tool() 83 | ,arrow_(NULL) 84 | { 85 | } 86 | 87 | MeshPoseTool::~MeshPoseTool() 88 | { 89 | if(arrow_) 90 | { 91 | delete arrow_; 92 | } 93 | } 94 | 95 | void MeshPoseTool::onInitialize() 96 | { 97 | arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); 98 | arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); 99 | arrow_->getSceneNode()->setVisible(false); 100 | } 101 | 102 | void MeshPoseTool::activate() 103 | { 104 | setStatus("Click and on a mesh_msgs::TriangleMesh to set the position and drag the mouse for the orientation."); 105 | state_ = Position; 106 | } 107 | 108 | void MeshPoseTool::setColor(float r, float g, float b, float a) 109 | { 110 | arrow_->setColor(r, g, b, a); 111 | } 112 | 113 | void MeshPoseTool::deactivate() 114 | { 115 | arrow_->getSceneNode()->setVisible(false); 116 | } 117 | 118 | int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) 119 | { 120 | int flags = 0; 121 | 122 | if (event.leftDown()) 123 | { 124 | // TODO: check if there is a proper ROS2 equivalent 125 | // RCLCPP_ASSERT(state_ == Position); 126 | assert(state_ == Position); 127 | 128 | Ogre::Ray mouse_ray = getMouseEventRay(event); 129 | 130 | // Find intersection 131 | Ogre::Vector3 int_pos; 132 | Ogre::Vector3 int_normal; 133 | Intersection intersection; 134 | if(selectFace(context_, mouse_ray, intersection)) 135 | { 136 | pos_start_ = intersection.point; 137 | normal_start_ = intersection.normal; 138 | 139 | // Flip normal to camera 140 | Ogre::Vector3 cam_pos = event.panel->getViewController()->getCamera()->getRealPosition(); 141 | Ogre::Vector3 dir_to_cam = cam_pos - pos_start_; 142 | dir_to_cam.normalise(); 143 | // there should be a function doing the dot product 144 | float scalar = dir_to_cam.x * normal_start_.x 145 | + dir_to_cam.y * normal_start_.y 146 | + dir_to_cam.z * normal_start_.z; 147 | 148 | if(scalar < 0) 149 | { 150 | normal_start_ = -normal_start_; 151 | } 152 | 153 | arrow_->setPosition(pos_start_); 154 | state_ = Orientation; 155 | } 156 | } 157 | else if (event.type == QEvent::MouseMove && event.left()) 158 | { 159 | if (state_ == Orientation) 160 | { 161 | Ogre::Plane plane(normal_start_, pos_start_); 162 | Ogre::Ray mouse_ray = getMouseEventRay(event); 163 | 164 | // intersect with plane 165 | auto res = mouse_ray.intersects(plane); 166 | if(res.first) 167 | { 168 | // plane hit 169 | pos_last_ = mouse_ray.getPoint(res.second); 170 | if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) 171 | { 172 | // valid distance 173 | 174 | // right hand coordinate system 175 | // x to the right, y to the top, and -z into the scene 176 | // arrow foreward negative z 177 | Ogre::Vector3 z_axis = -(pos_last_ - pos_start_).normalisedCopy(); 178 | Ogre::Vector3 y_axis = normal_start_; 179 | Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis).normalisedCopy(); 180 | 181 | arrow_->getSceneNode()->setVisible(true); 182 | arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); 183 | 184 | flags |= Render; 185 | } 186 | } 187 | 188 | } 189 | } 190 | else if (event.leftUp()) 191 | { 192 | if (state_ == Orientation) 193 | { 194 | Ogre::Plane plane(normal_start_, pos_start_); 195 | Ogre::Ray mouse_ray = getMouseEventRay(event); 196 | 197 | // intersect with plane 198 | auto res = mouse_ray.intersects(plane); 199 | if(res.first) 200 | { 201 | // plane hit 202 | pos_last_ = mouse_ray.getPoint(res.second); 203 | if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) 204 | { 205 | // valid distance 206 | 207 | // right hand coordinate system 208 | // x to the right, y to the top, and -z into the scene 209 | // arrow foreward negative z 210 | Ogre::Vector3 z_axis = -(pos_last_ - pos_start_).normalisedCopy(); 211 | Ogre::Vector3 y_axis = normal_start_; 212 | Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis).normalisedCopy(); 213 | 214 | arrow_->getSceneNode()->setVisible(true); 215 | arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); 216 | 217 | onPoseSet(pos_start_, Ogre::Quaternion(x_axis, y_axis, z_axis)); 218 | 219 | flags |= (Finished | Render); 220 | state_ = Position; 221 | } 222 | } 223 | } 224 | } 225 | 226 | return flags; 227 | } 228 | 229 | } // namespace rviz_mesh_tools_plugins 230 | -------------------------------------------------------------------------------- /rviz_mesh_tools_plugins/src/RvizFileProperty.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Robot Operating System code by the University of Osnabrück 5 | * Copyright (c) 2021, University of Osnabrück 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * 21 | * 3. Neither the name of the copyright holder nor the names of its 22 | * contributors may be used to endorse or promote products derived 23 | * from this software without specific prior written permission. 24 | * 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 30 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 33 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 34 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 35 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 36 | * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | * 38 | * 39 | * 40 | * RvizFileProperty.cpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Malte kleine Piening 46 | */ 47 | 48 | #include "rviz_mesh_tools_plugins/RvizFileProperty.hpp" 49 | 50 | #include 51 | 52 | namespace rviz_common 53 | { 54 | 55 | namespace properties 56 | { 57 | 58 | FileProperty::FileProperty( 59 | const QString& name, const QString& default_value, const QString& description, 60 | Property* parent, const char* changed_slot, QObject* receiver) 61 | : FilePickerProperty( 62 | name, default_value, description, 63 | parent, changed_slot, receiver) 64 | { 65 | } 66 | 67 | QWidget* FileProperty::createEditor( 68 | QWidget* parent, 69 | const QStyleOptionViewItem&) 70 | { 71 | QFileDialog* editor = new QFileDialog(nullptr); 72 | 73 | QStringList filenameFilters; 74 | filenameFilters << tr("*"); 75 | filenameFilters << tr("*.h5"); 76 | filenameFilters << tr("*.ply"); 77 | filenameFilters << tr("*.obj"); 78 | filenameFilters << tr("*.dae"); 79 | filenameFilters << tr("*.stl"); 80 | filenameFilters << tr("*.3d"); 81 | filenameFilters << tr("*.3ds"); 82 | filenameFilters << tr("*.fbx"); 83 | filenameFilters << tr("*.blend"); 84 | editor->setNameFilters(filenameFilters); 85 | 86 | editor->setViewMode(QFileDialog::Detail); 87 | 88 | if (editor->exec()) 89 | { 90 | QStringList fileNames = editor->selectedFiles(); 91 | if (fileNames.size() == 0) 92 | { 93 | setStdFilename(""); 94 | } 95 | else 96 | { 97 | setFilename(fileNames.at(0)); 98 | } 99 | } 100 | 101 | return nullptr; 102 | } 103 | 104 | } // namespace properties 105 | } // namespace rviz_common 106 | -------------------------------------------------------------------------------- /source_dependencies.yaml: -------------------------------------------------------------------------------- 1 | # Repositories in here contain dependencies of mesh_tools that are currently not avaiable via apt. 2 | # Every repository listed here will get cloned and built during CI runs. 3 | repositories: 4 | uos/lvr2: 5 | type: git 6 | url: https://github.com/uos/lvr2.git 7 | version: main 8 | --------------------------------------------------------------------------------