├── mesh_msgs ├── service │ ├── GetUUID.srv │ ├── GetLabeledClusters.srv │ ├── GetGeometry.srv │ ├── GetMaterials.srv │ ├── GetTexture.srv │ ├── GetVertexCosts.srv │ └── GetVertexColors.srv ├── msg │ ├── MeshFeatures.msg │ ├── MeshVertexCosts.msg │ ├── Feature.msg │ ├── MeshVertexTexCoords.msg │ ├── TriangleMeshStamped.msg │ ├── MeshFaceCluster.msg │ ├── MeshVertexColors.msg │ ├── VectorField.msg │ ├── VectorFieldStamped.msg │ ├── MeshMaterial.msg │ ├── TriangleIndices.msg │ ├── MeshTexture.msg │ ├── MeshGeometryStamped.msg │ ├── MeshMaterialsStamped.msg │ ├── MeshVertexColorsStamped.msg │ ├── MeshGeometry.msg │ ├── MeshVertexCostsStamped.msg │ ├── MeshMaterials.msg │ ├── MeshFaceClusterStamped.msg │ └── TriangleMesh.msg ├── README.md ├── CHANGELOG.rst ├── package.xml └── CMakeLists.txt ├── rviz_map_plugin ├── README.md ├── icons │ └── classes │ │ └── ClusterLabel.png ├── CHANGELOG.rst ├── package.xml ├── rviz_plugin.xml ├── CMakeLists.txt ├── include │ ├── ClusterLabelPanel.hpp │ ├── Types.hpp │ ├── kernels │ │ └── cast_rays.cl │ ├── ClusterLabelVisual.hpp │ ├── MapDisplay.hpp │ ├── MeshDisplay.hpp │ ├── ClusterLabelTool.hpp │ ├── ClusterLabelDisplay.hpp │ └── TexturedMeshVisual.hpp └── src │ ├── ClusterLabelPanel.cpp │ ├── ClusterLabelVisual.cpp │ └── MeshDisplay.cpp ├── label_manager ├── srv │ ├── GetLabelGroups.srv │ ├── DeleteLabel.srv │ └── GetLabeledClusterGroup.srv ├── README.md ├── CHANGELOG.rst ├── launch │ └── label_manager.launch ├── src │ ├── manager_node.cpp │ └── manager.cpp ├── CMakeLists.txt ├── package.xml └── include │ └── label_manager │ └── manager.h ├── doc └── rviz_triangle.png ├── .gitmodules ├── rviz_mesh_plugin ├── icons │ └── classes │ │ ├── MeshGoal.png │ │ └── ClusterLabel.png ├── CHANGELOG.rst ├── rviz_plugin.xml ├── package.xml ├── CMakeLists.txt └── src │ ├── mesh_goal_tool.h │ ├── mesh_pose_tool.h │ ├── mesh_goal_tool.cpp │ ├── face_selection_tool.h │ ├── trianglemesh_display.h │ ├── trianglemesh_visual.h │ └── mesh_pose_tool.cpp ├── mesh_tools ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── hdf5_map_io ├── CHANGELOG.rst ├── .gitmodules ├── package.xml ├── CMakeLists.txt └── include │ └── hdf5_map_io │ └── hdf5_map_io.h ├── mesh_msgs_hdf5 ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt └── include │ └── mesh_msgs_hdf5 │ └── mesh_msgs_hdf5.h ├── mesh_msgs_transform ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt ├── include │ └── mesh_msgs_transform │ │ └── transforms.h └── src │ └── transforms.cpp ├── .github └── workflows │ └── main.yml ├── LICENSE └── README.md /mesh_msgs/service/GetUUID.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string uuid 3 | -------------------------------------------------------------------------------- /rviz_map_plugin/README.md: -------------------------------------------------------------------------------- 1 | Plugin for viewing and labeling HDF5 mesh maps 2 | -------------------------------------------------------------------------------- /label_manager/srv/GetLabelGroups.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | string[] labels 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshFeatures.msg: -------------------------------------------------------------------------------- 1 | string map_uuid 2 | mesh_msgs/Feature[] features 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexCosts.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | float32[] costs 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/Feature.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point location 2 | std_msgs/Float32[] descriptor 3 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexTexCoords.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Type 2 | float32 u 3 | float32 v 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/TriangleMeshStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | mesh_msgs/TriangleMesh mesh 3 | -------------------------------------------------------------------------------- /mesh_msgs/service/GetLabeledClusters.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | MeshFaceCluster[] clusters 4 | -------------------------------------------------------------------------------- /doc/rviz_triangle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/mesh_tools/kinetic/doc/rviz_triangle.png -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshFaceCluster.msg: -------------------------------------------------------------------------------- 1 | #Cluster 2 | uint32[] face_indices 3 | #optional 4 | string label 5 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshVertexColors.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | std_msgs/ColorRGBA[] vertex_colors 3 | -------------------------------------------------------------------------------- /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/msg/MeshMaterial.msg: -------------------------------------------------------------------------------- 1 | uint32 texture_index 2 | std_msgs/ColorRGBA color 3 | bool has_texture 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/TriangleIndices.msg: -------------------------------------------------------------------------------- 1 | # Definition of a triangle's vertices 2 | uint32[3] vertex_indices 3 | -------------------------------------------------------------------------------- /label_manager/srv/DeleteLabel.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | string label 3 | --- 4 | mesh_msgs/MeshFaceCluster cluster 5 | -------------------------------------------------------------------------------- /mesh_msgs/service/GetGeometry.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshGeometryStamped mesh_geometry_stamped 4 | -------------------------------------------------------------------------------- /mesh_msgs/README.md: -------------------------------------------------------------------------------- 1 | # mesh_msgs 2 | 3 | This package contains various ROS message types to represent mesh data. 4 | -------------------------------------------------------------------------------- /mesh_msgs/service/GetMaterials.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshMaterialsStamped mesh_materials_stamped 4 | -------------------------------------------------------------------------------- /mesh_msgs/service/GetTexture.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | uint32 texture_index 3 | --- 4 | mesh_msgs/MeshTexture texture 5 | -------------------------------------------------------------------------------- /mesh_msgs/service/GetVertexCosts.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshVertexCostsStamped mesh_vertex_costs_stamped 4 | -------------------------------------------------------------------------------- /mesh_msgs/msg/MeshTexture.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | string uuid 3 | uint32 texture_index 4 | sensor_msgs/Image image 5 | -------------------------------------------------------------------------------- /mesh_msgs/service/GetVertexColors.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | --- 3 | mesh_msgs/MeshVertexColorsStamped mesh_vertex_colors_stamped 4 | -------------------------------------------------------------------------------- /label_manager/README.md: -------------------------------------------------------------------------------- 1 | # label_manager 2 | 3 | This package contains a ROS node for serving and persisting label information 4 | -------------------------------------------------------------------------------- /label_manager/srv/GetLabeledClusterGroup.srv: -------------------------------------------------------------------------------- 1 | string uuid 2 | string labelGroup 3 | --- 4 | mesh_msgs/MeshFaceCluster[] clusters 5 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "hdf5_map_io/ext/HighFive"] 2 | path = hdf5_map_io/ext/HighFive 3 | url = https://github.com/BlueBrain/HighFive.git 4 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/icons/classes/MeshGoal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/mesh_tools/kinetic/rviz_mesh_plugin/icons/classes/MeshGoal.png -------------------------------------------------------------------------------- /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/MeshMaterialsStamped.msg: -------------------------------------------------------------------------------- 1 | # Mesh Attribute Message 2 | std_msgs/Header header 3 | string uuid 4 | mesh_msgs/MeshMaterials mesh_materials 5 | -------------------------------------------------------------------------------- /mesh_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mesh_tools) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /rviz_map_plugin/icons/classes/ClusterLabel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/mesh_tools/kinetic/rviz_map_plugin/icons/classes/ClusterLabel.png -------------------------------------------------------------------------------- /rviz_mesh_plugin/icons/classes/ClusterLabel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ns130291/mesh_tools/kinetic/rviz_mesh_plugin/icons/classes/ClusterLabel.png -------------------------------------------------------------------------------- /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/MeshGeometry.msg: -------------------------------------------------------------------------------- 1 | # Mesh Geometry Message 2 | geometry_msgs/Point[] vertices 3 | geometry_msgs/Point[] vertex_normals 4 | mesh_msgs/TriangleIndices[] faces 5 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /hdf5_map_io/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hdf5_map_io 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 -------------------------------------------------------------------------------- /mesh_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | -------------------------------------------------------------------------------- /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_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_mesh_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | -------------------------------------------------------------------------------- /label_manager/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package label_manager 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | 9 | -------------------------------------------------------------------------------- /mesh_msgs_hdf5/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_msgs_hdf5 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /rviz_map_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_map_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_mesh_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | -------------------------------------------------------------------------------- /hdf5_map_io/.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "include/HighFive"] 2 | path = include/HighFive 3 | url = https://github.com/BlueBrain/HighFive.git 4 | [submodule "ext/HighFive"] 5 | path = ext/HighFive 6 | url = https://github.com/BlueBrain/HighFive.git 7 | -------------------------------------------------------------------------------- /mesh_msgs_transform/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mesh_msgs_transform 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.0 (2020-04-26) 6 | ------------------ 7 | * release version 1.0.0 8 | -------------------------------------------------------------------------------- /label_manager/launch/label_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /label_manager/src/manager_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * manager_node.cpp 3 | * 4 | */ 5 | 6 | #include 7 | #include "label_manager/manager.h" 8 | 9 | int main(int argc, char **argv) 10 | { 11 | ros::init(argc, argv, "label_manager"); 12 | ros::NodeHandle nodeHandle("~"); 13 | 14 | label_manager::LabelManager lm(nodeHandle); 15 | 16 | ros::spin(); 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - {ROS_DISTRO: melodic, ROS_REPO: testing} 11 | - {ROS_DISTRO: melodic, ROS_REPO: main} 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v2 15 | with: 16 | submodules: recursive 17 | - uses: 'ros-industrial/industrial_ci@master' 18 | env: ${{matrix.env}} 19 | -------------------------------------------------------------------------------- /hdf5_map_io/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hdf5_map_io 4 | 1.0.0 5 | The hdf5_map_io package 6 | Sebastian Pütz 7 | BSD-3 8 | http://wiki.ros.org/ros_mesh_tools/hdf5_map_io 9 | Sebastian Pütz 10 | 11 | catkin 12 | boost 13 | libhdf5-dev 14 | 15 | 16 | -------------------------------------------------------------------------------- /mesh_msgs/msg/TriangleMesh.msg: -------------------------------------------------------------------------------- 1 | ## Definition of a triangle mesh 2 | 3 | TriangleIndices[] triangles # list of triangles; the index values refer to positions in vertices (and vertex_normals, if given) 4 | geometry_msgs/Point[] vertices # the actual vertices that make up the mesh 5 | #optional: 6 | geometry_msgs/Point[] vertex_normals 7 | std_msgs/ColorRGBA[] vertex_colors 8 | std_msgs/ColorRGBA[] triangle_colors 9 | geometry_msgs/Point[] vertex_texture_coords 10 | mesh_msgs/MeshMaterial[] face_materials 11 | sensor_msgs/Image[] textures 12 | mesh_msgs/MeshFaceCluster[] clusters 13 | -------------------------------------------------------------------------------- /mesh_msgs_hdf5/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_msgs_hdf5 4 | 1.0.0 5 | Read and write mesh_msgs to and from hdf5 6 | Sebastian Pütz 7 | http://wiki.ros.org/ros_mesh_tools/mesh_msgs_hdf5 8 | BSD-3 9 | Sebastian Pütz 10 | 11 | mesh_msgs 12 | hdf5_map_io 13 | label_manager 14 | 15 | mesh_msgs 16 | hdf5_map_io 17 | label_manager 18 | 19 | catkin 20 | 21 | 22 | -------------------------------------------------------------------------------- /mesh_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_tools 4 | 1.0.0 5 | The mesh_tools package 6 | Sebastian Pütz 7 | BSD-3 8 | http://wiki.ros.org/mesh_tools 9 | Sebastian Pütz 10 | 11 | hdf5_map_io 12 | mesh_msgs 13 | mesh_msgs_transform 14 | rviz_map_plugin 15 | rviz_mesh_plugin 16 | label_manager 17 | 18 | catkin 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /mesh_msgs_transform/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_msgs_transform 4 | 1.0.0 5 | Methods to transform mesh_msgs 6 | Sebastian Pütz 7 | BSD-3 8 | http://wiki.ros.org/ros_mesh_tools/mesh_msgs_transform 9 | Sebastian Pütz 10 | 11 | tf 12 | mesh_msgs 13 | geometry_msgs 14 | eigen 15 | 16 | tf 17 | mesh_msgs 18 | geometry_msgs 19 | eigen 20 | 21 | catkin 22 | 23 | 24 | -------------------------------------------------------------------------------- /mesh_msgs_transform/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mesh_msgs_transform) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | tf 6 | geometry_msgs 7 | mesh_msgs 8 | ) 9 | 10 | find_package(Eigen3 REQUIRED) 11 | 12 | include_directories( 13 | include 14 | ${catkin_INCLUDE_DIRS} 15 | ${EIGEN3_INCLUDE_DIRS} 16 | ) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | LIBRARIES mesh_msgs_transform 21 | CATKIN_DEPENDS tf geometry_msgs mesh_msgs 22 | ) 23 | 24 | add_library(mesh_msgs_transform 25 | src/transforms.cpp 26 | ) 27 | 28 | target_link_libraries(mesh_msgs_transform 29 | ${catkin_LIBRARIES} 30 | ) 31 | 32 | add_dependencies(mesh_msgs_transform ${catkin_EXPORTED_TARGETS}) 33 | 34 | install(TARGETS mesh_msgs_transform 35 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | ) 37 | 38 | install(DIRECTORY include/${PROJECT_NAME}/ 39 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 40 | ) 41 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/rviz_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Displays 'mesh_msgs/TriangleMesh' messages. 7 | 8 | mesh_msgs/TriangleMeshStamped 9 | 10 | 13 | 14 | Displays 'mesh_msgs' messages with textures. 15 | 16 | 17 | 20 | 21 | Pick Faces from a mesh from 'mesh_msgs' and get their IDs. 22 | 23 | 24 | 27 | 28 | Select a goal on a mesh. 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mesh_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_msgs 4 | 1.0.0 5 | Various Message Types for Mesh Data. 6 | Sebastian Pütz 7 | Sebastian Pütz 8 | Henning Deeken 9 | Jan Philipp Vogtherr 10 | Rasmus Diederichsen 11 | Christian Swan 12 | 13 | http://wiki.ros.org/ros_mesh_tools/mesh_msgs 14 | BSD-3 15 | 16 | catkin 17 | 18 | roscpp 19 | message_generation 20 | geometry_msgs 21 | sensor_msgs 22 | std_msgs 23 | 24 | roscpp 25 | message_runtime 26 | geometry_msgs 27 | sensor_msgs 28 | std_msgs 29 | 30 | -------------------------------------------------------------------------------- /hdf5_map_io/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hdf5_map_io) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 6 | 7 | find_package(catkin REQUIRED) 8 | find_package(HDF5 REQUIRED COMPONENTS C CXX HL) 9 | 10 | catkin_package( 11 | INCLUDE_DIRS include ext/HighFive/include 12 | LIBRARIES hdf5_map_io 13 | DEPENDS HDF5 14 | ) 15 | 16 | # HighFive 17 | set(HIGHFIVE_EXAMPLES FALSE) 18 | set(HIGHFIVE_UNIT_TESTS FALSE) 19 | add_subdirectory(ext/HighFive) 20 | link_directories(${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES}) 21 | 22 | include_directories( 23 | include 24 | ${HDF5_INCLUDE_DIRS} 25 | ext/HighFive/include 26 | ) 27 | 28 | add_library(${PROJECT_NAME} 29 | src/hdf5_map_io.cpp 30 | ) 31 | 32 | link_libraries(${PROJECT_NAME} 33 | ${HDF5_LIBRARIES} 34 | ${HDF5_C_LIBRARIES} 35 | ) 36 | 37 | install(TARGETS ${PROJECT_NAME} 38 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | 43 | install(DIRECTORY include/${PROJECT_NAME}/ 44 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 45 | FILES_MATCHING PATTERN "*.h" 46 | PATTERN ".svn" EXCLUDE 47 | ) 48 | 49 | -------------------------------------------------------------------------------- /label_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(label_manager) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | actionlib 6 | actionlib_msgs 7 | genmsg 8 | mesh_msgs 9 | message_generation 10 | roscpp 11 | sensor_msgs 12 | std_msgs 13 | tf 14 | ) 15 | 16 | 17 | add_service_files(FILES 18 | DeleteLabel.srv 19 | GetLabelGroups.srv 20 | GetLabeledClusterGroup.srv 21 | ) 22 | 23 | generate_messages(DEPENDENCIES 24 | mesh_msgs 25 | std_msgs 26 | ) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | CATKIN_DEPENDS 31 | actionlib actionlib_msgs genmsg mesh_msgs message_generation message_runtime roscpp sensor_msgs std_msgs tf 32 | ) 33 | 34 | include_directories( 35 | include ${catkin_INCLUDE_DIRS} 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | 40 | add_executable(${PROJECT_NAME} 41 | src/manager.cpp 42 | src/manager_node.cpp) 43 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 44 | target_link_libraries(${PROJECT_NAME} 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | 54 | -------------------------------------------------------------------------------- /mesh_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mesh_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | message_generation 7 | roscpp 8 | sensor_msgs 9 | std_msgs 10 | ) 11 | 12 | add_message_files( 13 | FILES 14 | MeshFaceCluster.msg 15 | MeshFaceClusterStamped.msg 16 | Feature.msg 17 | MeshMaterial.msg 18 | MeshFeatures.msg 19 | MeshGeometry.msg 20 | MeshGeometryStamped.msg 21 | MeshMaterials.msg 22 | MeshMaterialsStamped.msg 23 | MeshVertexColors.msg 24 | MeshVertexColorsStamped.msg 25 | MeshVertexCosts.msg 26 | MeshVertexCostsStamped.msg 27 | MeshTexture.msg 28 | TriangleIndices.msg 29 | TriangleMesh.msg 30 | TriangleMeshStamped.msg 31 | VectorField.msg 32 | VectorFieldStamped.msg 33 | MeshVertexTexCoords.msg 34 | ) 35 | 36 | add_service_files( 37 | DIRECTORY 38 | service 39 | FILES 40 | GetGeometry.srv 41 | GetLabeledClusters.srv 42 | GetMaterials.srv 43 | GetTexture.srv 44 | GetUUID.srv 45 | GetVertexColors.srv 46 | GetVertexCosts.srv 47 | ) 48 | 49 | generate_messages( 50 | DEPENDENCIES 51 | geometry_msgs 52 | sensor_msgs 53 | std_msgs 54 | ) 55 | 56 | catkin_package( 57 | INCLUDE_DIRS 58 | CATKIN_DEPENDS 59 | geometry_msgs 60 | message_runtime 61 | roscpp 62 | sensor_msgs 63 | std_msgs 64 | ) 65 | -------------------------------------------------------------------------------- /label_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | label_manager 3 | 4 | Serving and persisting label information 5 | 6 | 1.0.0 7 | Sebastian Pütz 8 | 9 | http://wiki.ros.org/ros_mesh_tools/label_manager 10 | 11 | Sebastian Pütz 12 | Jan Philipp Vogtherr 13 | 14 | BSD-3 15 | 16 | actionlib_msgs 17 | actionlib 18 | genmsg 19 | mesh_msgs 20 | message_generation 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | tf 25 | 26 | actionlib_msgs 27 | actionlib 28 | genmsg 29 | mesh_msgs 30 | message_generation 31 | message_runtime 32 | roscpp 33 | sensor_msgs 34 | std_msgs 35 | tf 36 | 37 | catkin 38 | 39 | 40 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_mesh_plugin 3 | 4 | RViz display types and tools for the mesh_msgs package. 5 | 6 | 1.0.0 7 | Sebastian Pütz 8 | Sebastian Pütz 9 | Henning Deeken 10 | Marcel Mrozinski 11 | Kristin Schmidt 12 | Jan Philipp Vogtherr 13 | catkin 14 | 15 | BSD-3 16 | http://wiki.ros.org/ros_mesh_tools/rviz_mesh_plugin 17 | qtbase5-dev 18 | roscpp 19 | rviz 20 | std_msgs 21 | mesh_msgs 22 | tf2_ros 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | roscpp 27 | rviz 28 | std_msgs 29 | mesh_msgs 30 | tf2_ros 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /rviz_map_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_map_plugin 3 | 4 | RViz display types and tools for the mesh_msgs package. 5 | 6 | 1.0.0 7 | Sebastian Pütz 8 | Sebastian Pütz 9 | Kristin Schmidt 10 | Jan Philipp Vogtherr 11 | catkin 12 | 13 | BSD-3 14 | http://ros.org/wiki/mash_tools/rviz_map_plugin 15 | qtbase5-dev 16 | roscpp 17 | rviz 18 | std_msgs 19 | mesh_msgs 20 | hdf5_map_io 21 | ocl-icd-opencl-dev 22 | opencl-headers 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | roscpp 27 | rviz 28 | std_msgs 29 | mesh_msgs 30 | hdf5_map_io 31 | ocl-icd-opencl-dev 32 | opencl-headers 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /rviz_map_plugin/rviz_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 9 | 12 | 13 | A panel widget allowing creation of cluster labels. 14 | 15 | 16 | 19 | 20 | A tool allowing selection of clusters. 21 | 22 | 23 | 26 | 27 | Displays labeled clusters of a map. (Don't use without Map3D) 28 | 29 | 30 | 33 | 34 | Displays a map with labeled clusters. 35 | 36 | 37 | 40 | 41 | Displays the mesh of a map. (Don't use without Map3D) 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /mesh_msgs_hdf5/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | 3 | project(mesh_msgs_hdf5) 4 | 5 | set(PACKAGE_DEPENDENCIES 6 | mesh_msgs 7 | hdf5_map_io 8 | label_manager 9 | ) 10 | 11 | find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) 12 | find_package(HDF5 REQUIRED COMPONENTS C CXX HL) 13 | 14 | ### compile with c++11 15 | if ("${CMAKE_VERSION}" VERSION_LESS "3.1") 16 | if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 17 | set(CMAKE_CXX_FLAGS "--std=gnu++14 ${CMAKE_CXX_FLAGS}") 18 | endif () 19 | else () 20 | set(CMAKE_CXX_STANDARD 14) 21 | endif () 22 | 23 | include_directories( 24 | include 25 | ) 26 | 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ${HDF5_INCLUDE_DIRS} 30 | ) 31 | 32 | catkin_package( 33 | CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} 34 | INCLUDE_DIRS include 35 | DEPENDS HDF5 36 | LIBRARIES ${PROJECT_NAME} 37 | ) 38 | 39 | add_library(${PROJECT_NAME} 40 | src/mesh_msgs_hdf5.cpp 41 | ) 42 | 43 | link_directories(${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES}) 44 | 45 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 46 | 47 | #add_executable(${PROJECT_NAME}_node src/hdf5_to_msg_node.cpp) 48 | #target_link_libraries(${PROJECT_NAME}_node 49 | # ${catkin_LIBRARIES} 50 | # ${HDF5_LIBRARIES} 51 | #) 52 | 53 | #install( 54 | # DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 55 | #) 56 | 57 | install( 58 | TARGETS ${PROJECT_NAME}# ${PROJECT_NAME}_node 59 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 62 | ) 63 | 64 | install( 65 | DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | -------------------------------------------------------------------------------- /label_manager/include/label_manager/manager.h: -------------------------------------------------------------------------------- 1 | #ifndef LABEL_MANAGER_H_ 2 | #define LABEL_MANAGER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace label_manager 14 | { 15 | 16 | class LabelManager 17 | { 18 | public: 19 | LabelManager(ros::NodeHandle& nodeHandle); 20 | 21 | private: 22 | ros::NodeHandle nh; 23 | ros::Subscriber clusterLabelSub; 24 | ros::Publisher newClusterLabelPub; 25 | ros::ServiceServer srv_get_labeled_clusters; 26 | ros::ServiceServer srv_get_label_groups; 27 | ros::ServiceServer srv_get_labeled_cluster_group; 28 | ros::ServiceServer srv_delete_label; 29 | 30 | std::string folderPath; 31 | 32 | void clusterLabelCallback(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg); 33 | bool service_getLabeledClusters( 34 | mesh_msgs::GetLabeledClusters::Request& req, 35 | mesh_msgs::GetLabeledClusters::Response& res); 36 | bool service_getLabelGroups( 37 | label_manager::GetLabelGroups::Request& req, 38 | label_manager::GetLabelGroups::Response& res); 39 | bool service_getLabeledClusterGroup( 40 | label_manager::GetLabeledClusterGroup::Request& req, 41 | label_manager::GetLabeledClusterGroup::Response& res); 42 | bool service_deleteLabel( 43 | label_manager::DeleteLabel::Request& req, 44 | label_manager::DeleteLabel::Response& res); 45 | 46 | bool writeIndicesToFile(const std::string& fileName, const std::vector& indices, const bool append); 47 | std::vector readIndicesFromFile(const std::string& fileName); 48 | std::string getFileName(const std::string& uuid, const std::string& label); 49 | }; 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_mesh_plugin) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 6 | 7 | 8 | set(THIS_PACKAGE_ROS_DEPS 9 | roscpp 10 | rviz 11 | std_msgs 12 | mesh_msgs 13 | ) 14 | 15 | find_package(catkin REQUIRED COMPONENTS 16 | ${THIS_PACKAGE_ROS_DEPS} 17 | ) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS std_msgs mesh_msgs 21 | ) 22 | 23 | include_directories(${catkin_INCLUDE_DIRS}) 24 | 25 | ## This setting causes Qt's "MOC" generation to happen automatically. 26 | set(CMAKE_AUTOMOC ON) 27 | 28 | if(rviz_QT_VERSION VERSION_LESS "5") 29 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 30 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 31 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 32 | include(${QT_USE_FILE}) 33 | else() 34 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 35 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 36 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 37 | set(QT_LIBRARIES Qt5::Widgets) 38 | endif() 39 | 40 | add_definitions(-DQT_NO_KEYWORDS) 41 | 42 | set(SOURCE_FILES 43 | src/trianglemesh_display.cpp 44 | src/trianglemesh_visual.cpp 45 | src/textured_mesh_display.cpp 46 | src/textured_mesh_visual.cpp 47 | src/face_selection_tool.cpp 48 | src/mesh_pose_tool.cpp 49 | src/mesh_goal_tool.cpp 50 | ) 51 | 52 | add_library(rviz_mesh_plugin ${SOURCE_FILES}) 53 | target_link_libraries(rviz_mesh_plugin ${QT_LIBRARIES} ${catkin_LIBRARIES}) 54 | add_dependencies(rviz_mesh_plugin ${catkin_EXPORTED_TARGETS}) 55 | 56 | install(TARGETS rviz_mesh_plugin 57 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 60 | ) 61 | install(FILES 62 | rviz_plugin.xml 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 64 | 65 | install(DIRECTORY icons/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 67 | -------------------------------------------------------------------------------- /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 | #include 52 | 53 | namespace mesh_msgs_transform{ 54 | 55 | bool transformTriangleMeshNoTime( 56 | const std::string& target_frame, 57 | const mesh_msgs::TriangleMeshStamped& mesh_in, 58 | const std::string& fixed_frame, 59 | mesh_msgs::TriangleMeshStamped& mesh_out, 60 | const tf::TransformListener& tf_listener 61 | ); 62 | 63 | bool transformGeometryMeshNoTime( 64 | const std::string& target_frame, 65 | const mesh_msgs::MeshGeometryStamped& mesh_in, 66 | const std::string& fixed_frame, 67 | mesh_msgs::MeshGeometryStamped& mesh_out, 68 | const tf::TransformListener& tf_listener 69 | ); 70 | 71 | bool transformTriangleMesh( 72 | const std::string& target_frame, 73 | const ros::Time& target_time, 74 | const mesh_msgs::TriangleMeshStamped& mesh_in, 75 | const std::string& fixed_frame, 76 | mesh_msgs::TriangleMeshStamped& mesh_out, 77 | const tf::TransformListener& tf_listener 78 | ); 79 | 80 | } 81 | 82 | #endif /* transforms.h */ 83 | -------------------------------------------------------------------------------- /rviz_map_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_map_plugin) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 6 | 7 | set(THIS_PACKAGE_ROS_DEPS 8 | roscpp 9 | rviz 10 | std_msgs 11 | mesh_msgs 12 | hdf5_map_io 13 | ) 14 | 15 | find_package(catkin REQUIRED COMPONENTS 16 | ${THIS_PACKAGE_ROS_DEPS} 17 | ) 18 | 19 | find_package(MPI REQUIRED) 20 | find_package(Boost REQUIRED COMPONENTS system) 21 | find_package(HDF5 REQUIRED COMPONENTS C CXX HL) 22 | find_package(OpenCL 2 REQUIRED) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} 26 | DEPENDS Boost OpenCL HDF5 OpenCL MPI 27 | ) 28 | 29 | include_directories( 30 | include 31 | ${catkin_INCLUDE_DIRS} 32 | ${Boost_INCLUDE_DIRS} 33 | ${HDF5_INCLUDE_DIRS} 34 | ${OpenCL_INCLUDE_DIRS} 35 | ${MPI_INCLUDE_PATH} 36 | /usr/include 37 | ) 38 | 39 | link_directories(${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES}) 40 | 41 | ## This setting causes Qt's "MOC" generation to happen automatically. 42 | set(CMAKE_AUTOMOC ON) 43 | 44 | if("${rviz_QT_VERSION}" VERSION_LESS "5") 45 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 46 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 47 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 48 | include(${QT_USE_FILE}) 49 | else() 50 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 51 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 52 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 53 | set(QT_LIBRARIES Qt5::Widgets) 54 | endif() 55 | 56 | add_definitions(-DQT_NO_KEYWORDS) 57 | 58 | set(SOURCE_FILES 59 | src/ClusterLabelDisplay.cpp 60 | src/ClusterLabelPanel.cpp 61 | src/ClusterLabelTool.cpp 62 | src/ClusterLabelVisual.cpp 63 | src/MapDisplay.cpp 64 | src/MeshDisplay.cpp 65 | src/TexturedMeshVisual.cpp 66 | ) 67 | 68 | set(HEADER_FILES 69 | include/ClusterLabelDisplay.hpp 70 | include/ClusterLabelPanel.hpp 71 | include/ClusterLabelVisual.hpp 72 | include/MapDisplay.hpp 73 | include/MeshDisplay.hpp 74 | include/TexturedMeshVisual.hpp 75 | include/ClusterLabelTool.hpp 76 | include/CLUtil.hpp 77 | ) 78 | 79 | add_library(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES} ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES}) 80 | 81 | target_link_libraries(${PROJECT_NAME} 82 | ${QT_LIBRARIES} 83 | ${catkin_LIBRARIES} 84 | ${HDF5_LIBRARIES} 85 | ${HDF5_HL_LIBRARIES} 86 | ${OpenCL_LIBRARIES} 87 | ${MPI_C_LIBRARIES} 88 | ) 89 | 90 | add_dependencies(${PROJECT_NAME} 91 | ${catkin_EXPORTED_TARGETS} 92 | ${HDF5_LIBRARIES} 93 | ${HDF5_HL_LIBRARIES} 94 | ${OpenCL_LIBRARIES} 95 | ) 96 | 97 | install(TARGETS ${PROJECT_NAME} 98 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 99 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 100 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 101 | ) 102 | install(FILES 103 | rviz_plugin.xml 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 105 | 106 | install(DIRECTORY icons/ 107 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 108 | 109 | get_property(dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) 110 | foreach(dir ${dirs}) 111 | message(STATUS "dir='${dir}'") 112 | endforeach() 113 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/mesh_goal_tool.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 | * mesh_goal_tool.h 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | #ifndef RVIZ_MESH_PLUGIN__MESH_GOAL_TOOL_H_ 46 | #define RVIZ_MESH_PLUGIN__MESH_GOAL_TOOL_H_ 47 | 48 | #include "mesh_pose_tool.h" 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 55 | #include 56 | #endif 57 | 58 | namespace rviz_mesh_plugin{ 59 | 60 | /** 61 | * @class MeshGoalTool 62 | * @brief Tool for publishing a goal within a mesh 63 | */ 64 | class MeshGoalTool : public MeshPoseTool{ 65 | Q_OBJECT 66 | public: 67 | 68 | /** 69 | * @brief Constructor 70 | */ 71 | MeshGoalTool(); 72 | 73 | /** 74 | * @brief Callback that is executed when tool is initialized 75 | */ 76 | virtual void onInitialize(); 77 | 78 | private Q_SLOTS: 79 | 80 | /** 81 | * @brief Updates the topic on which the goal will be published 82 | */ 83 | void updateTopic(); 84 | 85 | protected: 86 | 87 | /** 88 | * @brief When goal is set, publish result 89 | * @param position Position 90 | * @param orientation Orientation 91 | */ 92 | virtual void onPoseSet( const Ogre::Vector3& position, const Ogre::Quaternion& orientation ); 93 | 94 | /// Property for the topic 95 | rviz::StringProperty* topic_property_; 96 | /// Switch bottom / top for selection 97 | rviz::BoolProperty* switch_bottom_top_; 98 | /// Publisher 99 | ros::Publisher pose_pub_; 100 | /// Node handle 101 | ros::NodeHandle nh_; 102 | 103 | }; 104 | 105 | } /* namespace rviz_mesh_plugin */ 106 | 107 | #endif /* mesh_goal_tool.h */ 108 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/mesh_pose_tool.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 | * mesh_pose_tool.h 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | 46 | #ifndef RVIZ_MESH_PLUGIN__MESH_POSE_TOOL_H_ 47 | #define RVIZ_MESH_PLUGIN__MESH_POSE_TOOL_H_ 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | namespace rviz_mesh_plugin{ 60 | 61 | class MeshPoseTool: public rviz::Tool{ 62 | public: 63 | MeshPoseTool(); 64 | virtual ~MeshPoseTool(); 65 | 66 | virtual void onInitialize(); 67 | 68 | virtual void activate(); 69 | virtual void deactivate(); 70 | 71 | virtual int processMouseEvent( rviz::ViewportMouseEvent& event ); 72 | 73 | protected: 74 | virtual void onPoseSet( const Ogre::Vector3& position, const Ogre::Quaternion& orientation ) = 0; 75 | 76 | void getRawManualObjectData( 77 | const Ogre::ManualObject *mesh, 78 | const size_t sectionNumber, 79 | size_t& vertexCount, 80 | Ogre::Vector3*& vertices, 81 | size_t& indexCount, 82 | unsigned long*& indices); 83 | 84 | bool getPositionAndOrientation( 85 | const Ogre::ManualObject* mesh, 86 | const Ogre::Ray &ray, 87 | Ogre::Vector3& position, 88 | Ogre::Vector3& orientation); 89 | 90 | bool selectTriangle( 91 | rviz::ViewportMouseEvent& event, 92 | Ogre::Vector3& position, 93 | Ogre::Vector3& orientation); 94 | 95 | rviz::Arrow* arrow_; 96 | enum State 97 | { 98 | Position, 99 | Orientation 100 | }; 101 | State state_; 102 | Ogre::Vector3 pos_; 103 | Ogre::Vector3 ori_; 104 | }; 105 | 106 | } /* namespace rviz_mesh_plugin */ 107 | 108 | #endif /* mesh_pose_tool.h */ 109 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/mesh_goal_tool.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_goal_tool.cpp 41 | * 42 | * author: Sebastian Pütz 43 | */ 44 | 45 | 46 | #include "mesh_goal_tool.h" 47 | 48 | #include 49 | PLUGINLIB_EXPORT_CLASS( rviz_mesh_plugin::MeshGoalTool, rviz::Tool ) 50 | 51 | namespace rviz_mesh_plugin{ 52 | MeshGoalTool::MeshGoalTool() 53 | { 54 | shortcut_key_ = 'm'; 55 | topic_property_ = new rviz::StringProperty( "Topic", "goal", 56 | "The topic on which to publish the mesh navigation goals.", 57 | getPropertyContainer(), SLOT(updateTopic()), this); 58 | 59 | switch_bottom_top_ = new rviz::BoolProperty("Switch Bottom/Top", 60 | false, "Enable to stwich the bottom and top.", 61 | getPropertyContainer()); 62 | 63 | } 64 | 65 | void MeshGoalTool::onInitialize() 66 | { 67 | MeshPoseTool::onInitialize(); 68 | setName( "Mesh Goal" ); 69 | updateTopic(); 70 | } 71 | 72 | void MeshGoalTool::updateTopic() 73 | { 74 | pose_pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); 75 | } 76 | 77 | void MeshGoalTool::onPoseSet( const Ogre::Vector3& position, const Ogre::Quaternion& orientation ){ 78 | geometry_msgs::PoseStamped msg; 79 | msg.pose.position.x = position.x; 80 | msg.pose.position.y = position.y; 81 | msg.pose.position.z = position.z; 82 | 83 | // ogreToRos(x,y,z) = (-z,-x,y) 84 | 85 | Ogre::Quaternion ros_orientation; 86 | 87 | if(switch_bottom_top_->getBool()) 88 | { 89 | ros_orientation.FromAxes( 90 | -orientation.zAxis(), 91 | orientation.xAxis(), 92 | -orientation.yAxis() 93 | ); 94 | } 95 | else 96 | { 97 | ros_orientation.FromAxes( 98 | -orientation.zAxis(), 99 | -orientation.xAxis(), 100 | orientation.yAxis() 101 | ); 102 | } 103 | 104 | msg.pose.orientation.x = ros_orientation.x; 105 | msg.pose.orientation.y = ros_orientation.y; 106 | msg.pose.orientation.z = ros_orientation.z; 107 | msg.pose.orientation.w = ros_orientation.w; 108 | 109 | msg.header.stamp = ros::Time::now(); 110 | msg.header.frame_id = context_->getFixedFrame().toStdString(); 111 | pose_pub_.publish(msg); 112 | } 113 | 114 | } 115 | -------------------------------------------------------------------------------- /mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_MSGS_HDF5_H_ 2 | #define MESH_MSGS_HDF5_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace mesh_msgs_hdf5 27 | { 28 | 29 | class hdf5_to_msg 30 | { 31 | 32 | public: 33 | hdf5_to_msg(); 34 | 35 | protected: 36 | // Mesh services 37 | bool service_getGeometry( 38 | mesh_msgs::GetGeometry::Request &req, 39 | mesh_msgs::GetGeometry::Response &res); 40 | bool service_getGeometryVertices( 41 | mesh_msgs::GetGeometry::Request &req, 42 | mesh_msgs::GetGeometry::Response &res); 43 | bool service_getGeometryFaces( 44 | mesh_msgs::GetGeometry::Request &req, 45 | mesh_msgs::GetGeometry::Response &res); 46 | bool service_getGeometryVertexnormals( 47 | mesh_msgs::GetGeometry::Request &req, 48 | mesh_msgs::GetGeometry::Response &res); 49 | 50 | bool service_getMaterials( 51 | mesh_msgs::GetMaterials::Request &req, 52 | mesh_msgs::GetMaterials::Response &res); 53 | bool service_getTexture( 54 | mesh_msgs::GetTexture::Request &req, 55 | mesh_msgs::GetTexture::Response &res); 56 | bool service_getUUID( 57 | mesh_msgs::GetUUID::Request &req, 58 | mesh_msgs::GetUUID::Response &res); 59 | bool service_getVertexColors( 60 | mesh_msgs::GetVertexColors::Request &req, 61 | mesh_msgs::GetVertexColors::Response &res); 62 | 63 | // Label manager services 64 | bool service_getLabeledClusters( 65 | mesh_msgs::GetLabeledClusters::Request &req, 66 | mesh_msgs::GetLabeledClusters::Response &res); 67 | bool service_getLabelGroups( 68 | label_manager::GetLabelGroups::Request &req, 69 | label_manager::GetLabelGroups::Response &res); 70 | bool service_getLabeledClusterGroup( 71 | label_manager::GetLabeledClusterGroup::Request &req, 72 | label_manager::GetLabeledClusterGroup::Response &res); 73 | bool service_deleteLabel( 74 | label_manager::DeleteLabel::Request &req, 75 | label_manager::DeleteLabel::Response &res); 76 | 77 | // Vertex costs 78 | bool service_getRoughness( 79 | mesh_msgs::GetVertexCosts::Request &req, 80 | mesh_msgs::GetVertexCosts::Response &res); 81 | bool service_getHeightDifference( 82 | mesh_msgs::GetVertexCosts::Request &req, 83 | mesh_msgs::GetVertexCosts::Response &res); 84 | 85 | void callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped::ConstPtr &msg); 86 | 87 | private: 88 | 89 | // Mesh message service servers 90 | ros::ServiceServer srv_get_geometry_; 91 | ros::ServiceServer srv_get_geometry_vertices_; 92 | ros::ServiceServer srv_get_geometry_faces_; 93 | ros::ServiceServer srv_get_geometry_vertex_normals_; 94 | ros::ServiceServer srv_get_materials_; 95 | ros::ServiceServer srv_get_texture_; 96 | ros::ServiceServer srv_get_uuid_; 97 | ros::ServiceServer srv_get_vertex_colors_; 98 | ros::ServiceServer srv_get_roughness_; 99 | ros::ServiceServer srv_get_height_difference_; 100 | 101 | // Label manager services and subs/pubs 102 | ros::Subscriber sub_cluster_label_; 103 | ros::Publisher pub_cluster_label_; 104 | ros::ServiceServer srv_get_labeled_clusters_; 105 | ros::ServiceServer srv_get_label_groups_; 106 | ros::ServiceServer srv_get_labeled_cluster_group_; 107 | ros::ServiceServer srv_delete_label_; 108 | 109 | // ROS 110 | ros::NodeHandle node_handle; 111 | 112 | // ROS parameter 113 | std::string inputFile; 114 | 115 | std::string mesh_uuid = "mesh"; 116 | 117 | }; 118 | 119 | } // end namespace 120 | 121 | #endif /* MESH_MSGS_HDF5_H_ */ 122 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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 | */ 48 | 49 | #pragma once 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | // Forward declarations 60 | class QLineEdit; 61 | class QPushButton; 62 | 63 | namespace rviz 64 | { 65 | class Tool; 66 | } 67 | 68 | namespace rviz_map_plugin 69 | { 70 | 71 | /** 72 | * @class ClusterLabelPanel 73 | * @brief Panel for interacting with the label tool 74 | */ 75 | class ClusterLabelPanel: public rviz::Panel 76 | { 77 | 78 | Q_OBJECT 79 | 80 | public: 81 | 82 | /** 83 | * @brief Constructor 84 | * @param parent This panel's parent, if available 85 | */ 86 | ClusterLabelPanel(QWidget* parent = 0); 87 | 88 | /** 89 | * @brief RViz callback on inizialize 90 | */ 91 | void onInitialize(); 92 | 93 | /** 94 | * @brief Load a configuration 95 | * @input config The configuration 96 | */ 97 | virtual void load(const rviz::Config& config); 98 | 99 | /** 100 | * @brief Save a configuration 101 | * @input config The configuration 102 | */ 103 | virtual void save(rviz::Config config) const; 104 | 105 | public Q_SLOTS: 106 | 107 | /** 108 | * @brief Set the name under which the current cluster will be saved 109 | * @param clusterName The new name 110 | */ 111 | void setClusterName(const QString& clusterName); 112 | 113 | /** 114 | * @brief Updates the current cluster name 115 | */ 116 | void updateClusterName(); 117 | 118 | /** 119 | * @brief Publishes the current cluster 120 | */ 121 | void publish(); 122 | 123 | /** 124 | * @brief Resets the current face selection state 125 | */ 126 | void resetFaces(); 127 | 128 | protected: 129 | 130 | /// Input for entering the cluster name 131 | QLineEdit* m_clusterNameEditor; 132 | /// Input for entering the output topic name 133 | QLineEdit* m_outputTopicEditor; 134 | 135 | /// Name of the cluster 136 | QString m_clusterName; 137 | 138 | /// Button for creating and publishing the cluster 139 | QPushButton* m_createClusterButton; 140 | /// Button for resetting the current faces in cluster 141 | QPushButton* m_resetFacesButton; 142 | 143 | /// Instance of the label tool from this package 144 | ClusterLabelTool* m_tool; 145 | 146 | /// Node handle 147 | ros::NodeHandle m_nodeHandle; 148 | 149 | }; 150 | 151 | } // end namespace rviz_map_plugin 152 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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 | */ 48 | 49 | #pragma once 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | namespace rviz_map_plugin { 57 | 58 | using std::vector; 59 | using std::string; 60 | using std::array; 61 | using boost::optional; 62 | 63 | /// Struct for normals 64 | struct Normal 65 | { 66 | float x; 67 | float y; 68 | float z; 69 | 70 | Normal(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {} 71 | }; 72 | 73 | /// Struct for texture coordinates 74 | struct TexCoords 75 | { 76 | float u; 77 | float v; 78 | 79 | TexCoords(float _u, float _v) : u(_u), v(_v) {} 80 | }; 81 | 82 | /// Struct for clusters 83 | struct Cluster 84 | { 85 | string name; 86 | vector faces; 87 | 88 | Cluster(string n, vector f) : name(n), faces(f) {} 89 | }; 90 | 91 | /// Struct for vertices 92 | struct Vertex 93 | { 94 | float x; 95 | float y; 96 | float z; 97 | }; 98 | 99 | /// Struct for faces 100 | struct Face 101 | { 102 | array vertexIndices; 103 | }; 104 | 105 | /// Struct for geometry 106 | struct Geometry 107 | { 108 | vector vertices; 109 | vector faces; 110 | 111 | Geometry() 112 | {} 113 | 114 | Geometry(vector v, vector f) 115 | { 116 | for (uint32_t i = 0; i < v.size() / 3; i++) 117 | { 118 | Vertex vertex; 119 | vertex.x = v[i * 3 + 0]; 120 | vertex.y = v[i * 3 + 1]; 121 | vertex.z = v[i * 3 + 2]; 122 | vertices.push_back(vertex); 123 | } 124 | 125 | for (uint32_t i = 0; i < f.size() / 3; i++) 126 | { 127 | Face face; 128 | face.vertexIndices[0] = f[i * 3 + 0]; 129 | face.vertexIndices[1] = f[i * 3 + 1]; 130 | face.vertexIndices[2] = f[i * 3 + 2]; 131 | faces.push_back(face); 132 | } 133 | } 134 | }; 135 | 136 | /// Struct for colors 137 | struct Color 138 | { 139 | float r; 140 | float g; 141 | float b; 142 | float a; 143 | 144 | Color() {} 145 | Color(float _r, float _g, float _b, float _a) : r(_r), g(_g), b(_b), a(_a) {} 146 | }; 147 | 148 | /// Struct for textures 149 | struct Texture 150 | { 151 | uint32_t width; 152 | uint32_t height; 153 | uint8_t channels; 154 | vector data; 155 | string pixelFormat; 156 | }; 157 | 158 | /// Struct for materials 159 | struct Material 160 | { 161 | optional textureIndex; 162 | Color color; 163 | vector faceIndices; 164 | }; 165 | 166 | 167 | } // namespace 168 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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_map_plugin/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 | 62 | namespace rviz_map_plugin 63 | { 64 | 65 | ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) 66 | : rviz::Panel(parent) 67 | { 68 | QHBoxLayout* clusterNameLayout = new QHBoxLayout(); 69 | clusterNameLayout->addWidget(new QLabel("Cluster Name:")); 70 | m_clusterNameEditor = new QLineEdit(); 71 | clusterNameLayout->addWidget(m_clusterNameEditor); 72 | 73 | m_createClusterButton = new QPushButton("Create and Label Cluster"); 74 | 75 | m_resetFacesButton = new QPushButton("Reset selected Faces"); 76 | 77 | QVBoxLayout* layout = new QVBoxLayout(); 78 | layout->addLayout(clusterNameLayout); 79 | layout->addWidget(m_createClusterButton); 80 | layout->addWidget(m_resetFacesButton); 81 | setLayout(layout); 82 | 83 | // Make signal/slot connections 84 | connect(m_clusterNameEditor, SIGNAL(editingFinished()), this, SLOT(updateClusterName())); 85 | 86 | connect(m_createClusterButton, SIGNAL(released()), this, SLOT(publish())); 87 | connect(m_resetFacesButton, SIGNAL(released()), this, SLOT(resetFaces())); 88 | } 89 | 90 | void ClusterLabelPanel::onInitialize() 91 | { 92 | // Check if the cluster label tool is already opened 93 | rviz::ToolManager* toolManager = vis_manager_->getToolManager(); 94 | QStringList toolClasses = toolManager->getToolClasses(); 95 | bool foundTool = false; 96 | for (int i = 0; i < toolClasses.size(); i++) 97 | { 98 | if (toolClasses.at(i).contains("ClusterLabel")) 99 | { 100 | m_tool = static_cast(toolManager->getTool(i)); 101 | foundTool = true; 102 | break; 103 | } 104 | } 105 | 106 | if (!foundTool) 107 | { 108 | m_tool = static_cast( 109 | vis_manager_->getToolManager()->addTool("rviz_map_plugin/ClusterLabel") 110 | ); 111 | } 112 | } 113 | 114 | 115 | void ClusterLabelPanel::setClusterName(const QString& clusterName) 116 | { 117 | m_clusterName = clusterName; 118 | Q_EMIT configChanged(); 119 | 120 | // Gray out the create cluster button when the cluster name is empty 121 | m_createClusterButton->setEnabled(m_clusterName != ""); 122 | } 123 | 124 | void ClusterLabelPanel::updateClusterName() 125 | { 126 | setClusterName(m_clusterNameEditor->text()); 127 | } 128 | 129 | void ClusterLabelPanel::publish() 130 | { 131 | ROS_INFO("Label Panel: Publish"); 132 | m_tool->publishLabel(m_clusterName.toStdString()); 133 | } 134 | 135 | void ClusterLabelPanel::resetFaces() 136 | { 137 | ROS_INFO("Label panel: Reset"); 138 | m_tool->resetFaces(); 139 | } 140 | 141 | void ClusterLabelPanel::save(rviz::Config config) const 142 | { 143 | rviz::Panel::save(config); 144 | config.mapSetValue("ClusterName", m_clusterName); 145 | } 146 | 147 | void ClusterLabelPanel::load(const rviz::Config& config) 148 | { 149 | rviz::Panel::load(config); 150 | QString clusterName; 151 | if (config.mapGetString("ClusterName", &clusterName)); 152 | { 153 | m_clusterNameEditor->setText(clusterName); 154 | updateClusterName(); 155 | } 156 | } 157 | 158 | } // End namespace rviz_map_plugin 159 | 160 | #include 161 | PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelPanel, rviz::Panel) 162 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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 | #pragma once 49 | 50 | #include 51 | 52 | #include 53 | 54 | #include 55 | 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | 66 | namespace Ogre 67 | { 68 | // Forward declaration 69 | class SceneNode; 70 | class Mesh; 71 | } // End namespace Ogre 72 | 73 | 74 | namespace rviz_map_plugin 75 | { 76 | 77 | /** 78 | * @class ClusterLabelVisual 79 | * @brief Visual to show a labeled cluster 80 | */ 81 | class ClusterLabelVisual 82 | { 83 | public: 84 | 85 | /** 86 | * @brief Constructor 87 | * 88 | * @param context The context that contains the display information. 89 | * @param labelId The label id (that has to be unique) 90 | */ 91 | ClusterLabelVisual( 92 | rviz::DisplayContext* context, 93 | std::string labelId 94 | ); 95 | 96 | /** 97 | * @brief Constructor 98 | * 99 | * @param context The context that contains the display information. 100 | * @param labelId The label id (that has to be unique) 101 | * @param geometry A shared pointer to the geometry to which the labels belong 102 | */ 103 | ClusterLabelVisual( 104 | rviz::DisplayContext* context, 105 | std::string labelId, 106 | std::shared_ptr geometry 107 | ); 108 | 109 | /** 110 | * @brief Destructor 111 | */ 112 | ~ClusterLabelVisual(); 113 | 114 | /** 115 | * @brief Disabling the copy constructor 116 | * 117 | * Each cluster label visual has a pointer to a SubMesh with a unique name, 118 | * when copying and then deleting one of the copies, the SubMesh would be deleted, thus the 119 | * pointers of the remaining copies would be invalid 120 | */ 121 | ClusterLabelVisual(const ClusterLabelVisual&) = delete; 122 | 123 | /** 124 | * @brief Disabling the copy assignment operator 125 | * 126 | * explanation: see deleted copy constructor ClusterLabelVisual(const ClusterLabelVisual&) 127 | */ 128 | ClusterLabelVisual& operator=(const ClusterLabelVisual&) = delete; 129 | 130 | /** 131 | * @brief Deletes the material 132 | */ 133 | void reset(); 134 | 135 | /** 136 | * @brief Sets the geometry 137 | * 138 | * @param geometry The geometry 139 | */ 140 | void setGeometry(std::shared_ptr geometry); 141 | 142 | /** 143 | * @brief Sets the faces, that are in the shown cluster 144 | * 145 | * @param faces A vector containing the face ids 146 | */ 147 | void setFacesInCluster(const std::vector& faces); 148 | 149 | /** 150 | * @brief Sets the color 151 | * 152 | * @param facesColor The color for the faces 153 | * @param alpha The opacity, defaults to 1.0f (fully opaque) 154 | */ 155 | void setColor(Ogre::ColourValue facesColor, float alpha = 1.0f); 156 | 157 | /** 158 | * @brief Returns the faces 159 | * 160 | * @return A vector containing the face ids 161 | */ 162 | std::vector getFaces() 163 | { 164 | return m_faces; 165 | }; 166 | 167 | private: 168 | 169 | void initMaterial(); 170 | 171 | rviz::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 | 186 | } // end namespace rviz_map_plugin 187 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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 | 50 | #ifndef MAP_DISPLAY_HPP 51 | #define MAP_DISPLAY_HPP 52 | 53 | #include 54 | 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 | #include 80 | 81 | #include 82 | #include 83 | #include 84 | 85 | #include 86 | #include 87 | #include 88 | #include 89 | #include 90 | #include 91 | #include 92 | #include 93 | #include 94 | 95 | #include 96 | 97 | #ifndef Q_MOC_RUN 98 | #include 99 | 100 | #include 101 | #include 102 | #include 103 | #include 104 | #include 105 | #include 106 | #include 107 | #include 108 | #include 109 | 110 | #endif 111 | 112 | #include 113 | #include 114 | 115 | namespace rviz 116 | { 117 | 118 | // Forward declaration 119 | class BoolProperty; 120 | class ColorProperty; 121 | class FloatProperty; 122 | class IntProperty; 123 | class EnumProperty; 124 | class StringProperty; 125 | 126 | } // End namespace rviz 127 | 128 | namespace rviz_map_plugin 129 | { 130 | 131 | using std::shared_ptr; 132 | using std::unique_ptr; 133 | using std::string; 134 | using std::vector; 135 | 136 | /** 137 | * @class MapDisplay 138 | * @brief Master display for the Mesh- and Cluster- subdisplays. THis implementation uses HDF5 as it's data source 139 | */ 140 | class MapDisplay: public rviz::Display 141 | { 142 | Q_OBJECT 143 | 144 | public: 145 | /** 146 | * @brief Constructor 147 | */ 148 | MapDisplay(); 149 | 150 | /** 151 | * @brief Destructor 152 | */ 153 | ~MapDisplay(); 154 | 155 | public Q_SLOTS: 156 | 157 | /** 158 | * @brief Saves a label to HDF5 159 | * @param cluster The cluster to be saved 160 | */ 161 | void saveLabel(Cluster cluster); 162 | 163 | /** 164 | * @brief Get the geometry 165 | * @return The geometry 166 | */ 167 | shared_ptr getGeometry(); 168 | 169 | private Q_SLOTS: 170 | 171 | /** 172 | * @brief Update the map, based on the current data state 173 | */ 174 | void updateMap(); 175 | 176 | private: 177 | 178 | /** 179 | * @brief RViz callback on initialize 180 | */ 181 | void onInitialize(); 182 | 183 | /** 184 | * @brief RViz callback on enable 185 | */ 186 | void onEnable(); 187 | 188 | /** 189 | * @brief RViz callback on disable 190 | */ 191 | void onDisable(); 192 | 193 | /** 194 | * @brief Read all data from the HDF5 file and save it in the member variables 195 | * @return true, if successful 196 | */ 197 | bool loadData(); 198 | 199 | /// Geometry 200 | shared_ptr m_geometry; 201 | /// Materials 202 | vector m_materials; 203 | /// Textures 204 | vector m_textures; 205 | /// Colors 206 | vector m_colors; 207 | /// Vertex normals 208 | vector m_normals; 209 | /// Texture coordinates 210 | vector m_texCoords; 211 | /// Clusters 212 | vector m_clusterList; 213 | 214 | /// Path to map file 215 | rviz::StringProperty* m_mapFilePath; 216 | 217 | /// Subdisplay: ClusterLabel (for showing the clusters) 218 | rviz_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; 219 | /// Subdisplay: MeshDisplay (for showing the mesh) 220 | rviz_map_plugin::MeshDisplay* m_meshDisplay; 221 | 222 | /** 223 | * @brief Create a RViz display from it's unique class_id 224 | * @param class_id The class ID 225 | * @return Pointer to RViz display 226 | */ 227 | rviz::Display* createDisplay(const QString& class_id); 228 | }; 229 | 230 | } // end namespace rviz_map_plugin 231 | 232 | #endif 233 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/face_selection_tool.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 | * face_selection_tool.h 41 | * 42 | * authors: Henning Deeken 43 | * Sebastian Pütz 44 | * Tristan Igelbrink 45 | * Johannes Heitmann 46 | * Marcel Mrozinski 54 | #include 55 | #include 56 | 57 | #include 58 | #include 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | #include 67 | #include 68 | #include 69 | 70 | #include 71 | #include 72 | #include 73 | 74 | #include 75 | #include 76 | #include 77 | #include 78 | 79 | #ifndef Q_MOC_RUN 80 | #include 81 | 82 | #include 83 | #include 84 | #include 85 | #include 86 | #include 87 | #include 88 | #include 89 | #include 90 | 91 | #endif 92 | 93 | namespace rviz{ 94 | class RosTopicProperty; 95 | } 96 | 97 | /** 98 | * 99 | *@class FaceSelectionTool 100 | * 101 | *@brief Implements a rviz tool for marking single faces in a mesh 102 | * 103 | * with this rviz tool the user can mark single faces in a displayed 104 | * OGRE mesh. The marking can be done by left click or with a selection 105 | * box 106 | * 107 | */ 108 | 109 | // OGRE stuff 110 | namespace Ogre 111 | { 112 | class SceneNode; 113 | class Vector3; 114 | } 115 | 116 | namespace rviz_mesh_plugin 117 | { 118 | class FaceSelectionTool: public rviz::Tool 119 | { 120 | Q_OBJECT 121 | public: 122 | 123 | FaceSelectionTool(); 124 | ~FaceSelectionTool(); 125 | virtual void onInitialize(); 126 | virtual void activate(); 127 | virtual void deactivate(); 128 | 129 | virtual int processKeyEvent(QKeyEvent* event, rviz::RenderPanel* panel); 130 | virtual int processMouseEvent(rviz::ViewportMouseEvent& event); 131 | 132 | static const float BOX_SIZE_TOLERANCE; 133 | static const size_t MAXIMUM_PICKED_FACES; 134 | 135 | void clearSelection(); 136 | 137 | bool areFacesSelected(); 138 | void getSelectedFaces(size_t goalSection, std::string regionLabel, mesh_msgs::TriangleMesh &meshMsg); 139 | 140 | 141 | private Q_SLOTS: 142 | void updateTopic(); 143 | 144 | private: 145 | 146 | void initNode(); 147 | void initOgre(); 148 | 149 | void updateSelectionMesh(); 150 | 151 | void meshCb(const mesh_msgs::TriangleMeshStamped::ConstPtr& mesh); 152 | 153 | void setTransform(const mesh_msgs::TriangleMeshStamped &mesh); 154 | void setReferenceMesh( mesh_msgs::TriangleMesh mesh); 155 | void getSegmentMesh( mesh_msgs::TriangleMesh& mesh); 156 | 157 | void selectSingleFace(rviz::ViewportMouseEvent& event); 158 | void deselectSingleFace(rviz::ViewportMouseEvent& event); 159 | 160 | bool singleRayQuery(rviz::ViewportMouseEvent& event, int num_results, Ogre::Ray& ray); 161 | 162 | void getIdentityOfSingleFace(Ogre::ManualObject* mesh, 163 | Ogre::Ray &ray, 164 | size_t &goalSection, 165 | size_t &goalIndex, 166 | Ogre::Real& dist); 167 | 168 | void getRawManualObjectData(Ogre::ManualObject *mesh, 169 | size_t sectionNumber, 170 | size_t &vertexCount, 171 | Ogre::Vector3* &vertices, 172 | size_t &indexCount, 173 | unsigned long* &indices); 174 | 175 | Ogre::SceneManager* scene_manager; 176 | Ogre::ManualObject* reference_mesh; 177 | Ogre::MaterialPtr reference_mesh_material; 178 | Ogre::ManualObject* segment_mesh; 179 | Ogre::MaterialPtr segment_mesh_material; 180 | Ogre::SceneNode* scene_node; 181 | 182 | rviz::RosTopicProperty* mesh_topic; 183 | 184 | std::map > m_goalFaces; 185 | 186 | bool m_singleSelect; 187 | bool m_singleDeselect; 188 | 189 | ros::NodeHandle n; 190 | ros::Subscriber mesh_sub; 191 | ros::Publisher mesh_pub; 192 | ros::Publisher id_pub; 193 | ros::Publisher goal_pub; 194 | int num_results; 195 | bool has_mesh; 196 | 197 | int reference_color_r = 0; 198 | int reference_color_g = 155; 199 | int reference_color_b = 155; 200 | float reference_color_a = 0.5; 201 | 202 | int segment_color_r = 0; 203 | int segment_color_g = 0; 204 | int segment_color_b = 255; 205 | float segment_color_a = 0.75; 206 | 207 | }; 208 | } 209 | #endif 210 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mesh Tools for Ubuntu 16.04 with ROS Kinetic 2 | 3 | ## Installation 4 | 5 | Install required packages 6 | 7 | ### Intel processor, no GPU 8 | 9 | sudo add-apt-repository ppa:intel-opencl/intel-opencl 10 | sudo apt update 11 | sudo apt install intel-opencl-icd 12 | 13 | ### All platforms 14 | 15 | sudo apt install ocl-icd-opencl-dev 16 | 17 | 18 | Inside `src` folder of your workspace 19 | 20 | git clone https://github.com/ns130291/mesh_tools.git 21 | cd mesh_tools 22 | git submodule update --init --recursive 23 | 24 | Build your workspace and source it 25 | 26 | ## Testing 27 | 28 | Open three terminals 29 | 30 | 1st terminal 31 | 32 | roscore 33 | 34 | 2nd terminal 35 | 36 | rostopic pub -r 1 /mesh2 mesh_msgs/TriangleMeshStamped "header: 37 | seq: 0 38 | stamp: 39 | secs: 0 40 | nsecs: 0 41 | frame_id: 'base_footprint' 42 | mesh: 43 | triangles: 44 | - vertex_indices: [0, 1, 2] 45 | vertices: 46 | - {x: 0.0, y: 0.0, z: 0.0} 47 | - {x: 1.0, y: 0.0, z: 0.0} 48 | - {x: 1.0, y: 1.0, z: 0.0}" 49 | 50 | 3rd terminal 51 | 52 | rviz 53 | 54 | Add a *TriangleMesh* Display and set it to topic `/mesh2`. Set *Fixed Frame* in *Global Options* to `base_footprint`. 55 | 56 | The rviz window should then show a green triangle: 57 | ![rviz triangle display](doc/rviz_triangle.png) 58 | 59 | ## Implemented changes 60 | 61 | * Changed tf API from tf2 to old tf API as rviz in ROS Kinetic still uses it. In ROS Melodic rviz was updated to use tf2. 62 | 63 | ___ 64 | 65 | 66 | # Mesh Tools 67 | 68 | We introduce a set of tools to make 3D environment mesh representations 69 | accessible and manageable in ROS. We provide RViz tools to visualize and 70 | annotate huge meshes in combination with generated textures and different 71 | cost layers, which are based on the geometric analyses of the environment, 72 | or which represent different sensor readings, e.g. RGB image or even 73 | hyper-spectral image textures. 74 | 75 | ## Introduction 76 | 77 | Over the last years, the Knowledge Based Systems Group has developed 78 | an extensive set of tools to automatically generate triangle meshes 79 | from 3D laser scans. These tools are implemented in the 80 | freely available Las Vegas Surface Reconstruction Toolkit 81 | (LVR) and have been successfully applied in different 82 | robotic contexts. 83 | 84 | With terrestrial and mobile laser scanning it is possible to create 3D 85 | point clouds of large environments in short time. However, the amount 86 | of collected data is too large to be processed on a mobile robot. To 87 | make this data available on mobile robots, we suggest to compute 3D 88 | triangle meshes from point cloud data as corresponding map 89 | representations. Such maps have several advantages 90 | over raw point cloud data and voxel representations. They overcome the 91 | discretization of voxel maps and deliver topologically connected 92 | surface representations, which ease segmentation, annotation and enable intuitive visualization. 93 | 94 | However, to make the maps accessible in ROS, corresponding message 95 | definitions and tools for storing, handling and editing such maps are 96 | required. 97 | 98 | ## Message Definitions 99 | 100 | 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: 101 | 102 | + **MeshGeometry(Stamped)**: Geometric structure of a triangle mesh using a buffer vertices, normals and faces 103 | + **MeshVertexColors(Stamped)**: Vertex color information of a corresponding mesh 104 | + **MeshVertexCosts(Stamped)**: Vertex cost information of a corresponding mesh coloring the mesh using a rainbow color scheme. 105 | + **ClusterLabel**: Grouping a set of triangles / faces of a corresponding mesh using the face indices, optional cluster label 106 | + **Texture**: Texture using an Image and an ID 107 | + **Material**: Color and optional texture ID 108 | + **VertexTexCoords**: Texture coordinate 109 | + **MeshMaterials(Stamped)**: Combining the materials with texture coordinates and clusters of a corresponding mesh 110 | + **Feature**: A feature at a specific location and its feature descriptor 111 | + **MeshFeatures**: List of features for a corresponding mesh 112 | 113 | ## RViz Plugins 114 | 115 | We developed several RViz plugins to display 3D meshes together 116 | with different information layers (RGB textures, semantic labels, 117 | trafficability, etc.). It is possible to render the received meshes in 118 | different modes (lighting, materials and wireframe). The supported 119 | modes are selectable based on availability in the tree view. For path 120 | planning, we also implemented an interactive tool to set navigation 121 | goal poses on the mesh surface. To generate semantic maps, we implemented an 122 | interactive tool that can be used to select connected clusters of 123 | triangles and assign semantic labels to them. Some screenshots and videos demonstrate these plugins (see Supplementary Material). 124 | 125 | ### RViz Mesh Plugin 126 | 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. 127 | 128 | ### RViz Map Plugin 129 | 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 *ClusterLanel Panel* allows to manage your clusters and label names as well as the corresponding colors. 130 | 131 | # Videos 132 | 133 | ## Mesh Cost Layer 134 | 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 135 | 136 | [![Mesh Cost Layer](http://img.youtube.com/vi/Ac1YLn88QGk/0.jpg)](http://www.youtube.com/watch?v=Ac1YLn88QGk) 137 | 138 | ## Mesh Textures 139 | This Video shows a textured dataset of the botanical garden of Osnabrück including footpaths 140 | 141 | [![Mesh Textures](http://img.youtube.com/vi/CF-WdXwx_zo/0.jpg)](http://www.youtube.com/watch?v=CF-WdXwx_zo) 142 | 143 | ## Label Tool 144 | 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 145 | 146 | [![Label Tool](http://img.youtube.com/vi/3IV2yo0D_CU/0.jpg)](http://www.youtube.com/watch?v=3IV2yo0D_CU) 147 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/MeshDisplay.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 | * MeshDisplay.hpp 41 | * 42 | * 43 | * authors: 44 | * 45 | * Kristin Schmidt 46 | * Jan Philipp Vogtherr 47 | */ 48 | 49 | 50 | #ifndef MESH_DISPLAY_HPP 51 | #define MESH_DISPLAY_HPP 52 | 53 | #include 54 | #include 55 | 56 | #include 57 | #include 58 | 59 | #include 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | 74 | #include 75 | #include 76 | #include 77 | 78 | #ifndef Q_MOC_RUN 79 | #include 80 | 81 | #include 82 | #include 83 | #include 84 | #include 85 | #include 86 | #include 87 | 88 | #endif 89 | 90 | namespace rviz 91 | { 92 | 93 | // Forward declaration 94 | class BoolProperty; 95 | class ColorProperty; 96 | class FloatProperty; 97 | class IntProperty; 98 | class EnumProperty; 99 | class StringProperty; 100 | 101 | } // End namespace rviz 102 | 103 | namespace rviz_map_plugin 104 | { 105 | 106 | using std::shared_ptr; 107 | using std::unique_ptr; 108 | using std::string; 109 | using std::vector; 110 | 111 | // Forward declaration 112 | class TexturedMeshVisual; 113 | 114 | 115 | /** 116 | * @class MeshDisplay 117 | * @brief Display for showing the mesh in different modes 118 | */ 119 | class MeshDisplay: public rviz::Display 120 | { 121 | Q_OBJECT 122 | 123 | public: 124 | 125 | /** 126 | * @brief Constructor 127 | */ 128 | MeshDisplay(); 129 | 130 | /** 131 | * @brief Destructor 132 | */ 133 | ~MeshDisplay(); 134 | 135 | /** 136 | * @brief Set the geometry 137 | * @param geometry The geometry 138 | */ 139 | void setGeometry(shared_ptr geometry); 140 | 141 | /** 142 | * @brief Set the vertex colors 143 | * @param vertexColors The vertex colors 144 | */ 145 | void setVertexColors(vector& vertexColors); 146 | 147 | /** 148 | * @brief Set the vertex normals 149 | * @param vertexNormals The vertex normals 150 | */ 151 | void setVertexNormals(vector& vertexNormals); 152 | 153 | /** 154 | * @brief Set the materials and texture coordinates 155 | * @param materials The materials 156 | * @param texCoords The texture coordinates 157 | */ 158 | void setMaterials(vector& materials, vector& texCoords); 159 | 160 | /** 161 | * @brief Add a texture 162 | * @param texture The texture 163 | * @param textureIndex The textures index 164 | */ 165 | void addTexture(Texture& texture, uint32_t textureIndex); 166 | 167 | /** 168 | * @brief RViz callback on enable 169 | */ 170 | void onEnable(); 171 | 172 | /** 173 | * @brief RViz callback on disable 174 | */ 175 | void onDisable(); 176 | 177 | private Q_SLOTS: 178 | 179 | /** 180 | * @brief Updates the mesh 181 | */ 182 | void updateMesh(); 183 | 184 | /** 185 | * @brief Updates the mesh wireframe 186 | */ 187 | void updateWireframe(); 188 | 189 | /** 190 | * @brief Update the mesh normals 191 | */ 192 | void updateNormals(); 193 | 194 | private: 195 | 196 | /** 197 | * @brief RViz callback on initialize 198 | */ 199 | void onInitialize(); 200 | 201 | /// Geometry data 202 | shared_ptr m_geometry; 203 | 204 | /// Visual data 205 | shared_ptr m_visual; 206 | 207 | /// Property to set wireframe color 208 | rviz::ColorProperty* m_wireframeColor; 209 | 210 | /// Property to set wireframe transparency 211 | rviz::FloatProperty* m_wireframeAlpha; 212 | 213 | /// Property to set faces color 214 | rviz::ColorProperty* m_facesColor; 215 | 216 | /// Property to set faces transparency 217 | rviz::FloatProperty* m_facesAlpha; 218 | 219 | /// Property to use the vertex colors 220 | rviz::BoolProperty* m_facesVertexColors; 221 | 222 | /// Property to use the triangle colors 223 | rviz::BoolProperty* m_facesTriangleColors; 224 | 225 | /// Property to set the size of the normals 226 | rviz::FloatProperty* m_scalingFactor; 227 | 228 | /// Property to set the color of the normals 229 | rviz::ColorProperty* m_normalsColor; 230 | 231 | /// Property to set the transparency of the normals 232 | rviz::FloatProperty* m_normalsAlpha; 233 | 234 | /// Property to select the display type 235 | rviz::EnumProperty* m_displayType; 236 | 237 | /// Property to select the wireframe 238 | rviz::BoolProperty* m_showWireframe; 239 | 240 | /// Property to select the normals 241 | rviz::BoolProperty* m_showNormals; 242 | 243 | /// Property to only show textured faces when texturizing is enabled 244 | rviz::BoolProperty* m_showTexturedFacesOnly; 245 | 246 | /// Will be set to true once the initial data has arrived 247 | bool has_data = false; 248 | 249 | }; 250 | 251 | } // end namespace rviz_map_plugin 252 | 253 | #endif 254 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/trianglemesh_display.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 | * trianglemesh_display.h 41 | * 42 | * 43 | * authors: 44 | * 45 | * Sebastian Pütz 46 | * Henning Deeken 47 | * Marcel Mrozinski 48 | * Nils Oesting 49 | */ 50 | 51 | #ifndef TRIANGLEMESH_DISPLAY_H 52 | #define TRIANGLEMESH_DISPLAY_H 53 | 54 | #include 55 | 56 | #ifndef Q_MOC_RUN 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include "trianglemesh_visual.h" 63 | #endif 64 | 65 | namespace rviz 66 | { 67 | 68 | // Forward declaration 69 | class BoolProperty; 70 | class ColorProperty; 71 | class FloatProperty; 72 | class IntProperty; 73 | class RosTopicProperty; 74 | class EnumProperty; 75 | 76 | } // End namespace rviz 77 | 78 | 79 | namespace rviz_mesh_plugin 80 | { 81 | 82 | // Forward declaration 83 | class TriangleMeshVisual; 84 | 85 | /** 86 | * @brief TriangleMeshDisplay 87 | * @brief Class to show options in rviz window. 88 | */ 89 | class TriangleMeshDisplay : public rviz::Display 90 | { 91 | Q_OBJECT 92 | public: 93 | 94 | /// Counter for the number of displays 95 | static size_t displayCounter; 96 | 97 | /** 98 | * @brief Constructor. 99 | */ 100 | TriangleMeshDisplay(); 101 | 102 | void setTopic(const QString &topic, const QString &datatype) override; 103 | 104 | /** 105 | * @brief Destructor. 106 | */ 107 | ~TriangleMeshDisplay(); 108 | 109 | protected: 110 | 111 | /** 112 | * @brief Initialises all nessessary things to get started. 113 | */ 114 | void onInitialize(); 115 | 116 | /** 117 | * @brief Clears whole stored data. 118 | */ 119 | void reset(); 120 | 121 | /** 122 | * @brief Set the topics to subscribe. 123 | */ 124 | void subscribe(); 125 | 126 | /** 127 | * @brief Unsubscribes all topics. 128 | */ 129 | void unsubscribe(); 130 | 131 | /** 132 | * @brief Calls subscribe() if display is enabled 133 | */ 134 | void onEnable(); 135 | 136 | /** 137 | * @brief Calls unsubscribe() and reset() if display is disabled. 138 | */ 139 | void onDisable(); 140 | 141 | /** 142 | * @brief Sets the fixed frame. 143 | */ 144 | void fixedFrameChanged(); 145 | 146 | /** 147 | * @brief Tests if messages are valid, calls processMessage(). 148 | * @param meshMsg Message containing geometry information 149 | */ 150 | void incomingMessage(const mesh_msgs::TriangleMeshStamped::ConstPtr& meshMsg); 151 | 152 | 153 | private Q_SLOTS: 154 | 155 | /** 156 | * @brief Updates material for each mesh displayed by trianglemesh_visual. 157 | */ 158 | void updateMesh(); 159 | 160 | /** 161 | * @brief Sets capacity of trianglemesh_visual. 162 | */ 163 | void updateMeshBufferSize(); 164 | 165 | /** 166 | * @brief Updates the subscribed topic. 167 | */ 168 | void updateTopic(); 169 | 170 | /** 171 | * @brief Updates the topic synchronizer 172 | */ 173 | void updateSynchronizer(); 174 | 175 | private: 176 | 177 | /** 178 | * @brief Sets data for trianglemesh_visual and updates the mesh. 179 | * @param meshMsg Message containing geometry information 180 | */ 181 | void processMessage(const mesh_msgs::TriangleMeshStamped::ConstPtr& meshMsg); 182 | 183 | /// Subscriber for meshMsg 184 | message_filters::Subscriber m_meshSubscriber; 185 | 186 | /// Messagefilter for meshMsg 187 | tf::MessageFilter* m_tfMeshFilter; 188 | 189 | /// Synchronizer for meshMsgs 190 | message_filters::Cache* m_synchronizer; 191 | 192 | /// Counter for the received messages 193 | uint32_t m_messagesReceived; 194 | 195 | /// Shared pointer to store the created objects of trianglemesh_visual 196 | boost::circular_buffer > m_meshVisuals; 197 | 198 | /// Counter for the meshes 199 | size_t m_meshCounter; 200 | 201 | /// DisplayID 202 | size_t m_displayID; 203 | 204 | /// Property to handle topic for meshMsg 205 | rviz::RosTopicProperty* m_meshTopic; 206 | 207 | /// Property to set meshBufferSize 208 | rviz::IntProperty* m_meshBufferSize; 209 | 210 | /// Property to set wireframe color 211 | rviz::ColorProperty* m_wireframeColor; 212 | 213 | /// Property to set wireframe transparency 214 | rviz::FloatProperty* m_wireframeAlpha; 215 | 216 | /// Property to set faces color 217 | rviz::ColorProperty* m_facesColor; 218 | 219 | /// Property to set faces transparency 220 | rviz::FloatProperty* m_facesAlpha; 221 | 222 | /// Property to use the vertex colors 223 | rviz::BoolProperty* m_facesVertexColors; 224 | 225 | /// Property to use the triangle colors 226 | rviz::BoolProperty* m_facesTriangleColors; 227 | 228 | /// Property to set the size of the normals 229 | rviz::FloatProperty* m_scalingFactor; 230 | 231 | /// Property to set the color of the normals 232 | rviz::ColorProperty* m_normalsColor; 233 | 234 | /// Property to set the transparency of the normals 235 | rviz::FloatProperty* m_normalsAlpha; 236 | 237 | /// Property to select the display type 238 | rviz::EnumProperty* m_displayType; 239 | 240 | /// Property to select the wireframe 241 | rviz::BoolProperty* m_showWireframe; 242 | 243 | /// Property to select the normals 244 | rviz::BoolProperty* m_showNormals; 245 | 246 | }; 247 | } // End namespace rviz_mesh_plugin 248 | 249 | #endif 250 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/trianglemesh_visual.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 | * trianglemesh_visual.h 41 | * 42 | * 43 | * authors: 44 | * 45 | * Sebastian Pütz 46 | * Henning Deeken 47 | * Marcel Mrozinski 48 | * Nils Oesting 49 | */ 50 | 51 | #ifndef TRIANGLEMESH_VISUAL_H 52 | #define TRIANGLEMESH_VISUAL_H 53 | 54 | #include 55 | 56 | #include 57 | #include 58 | 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | namespace Ogre 67 | { 68 | 69 | // Forward declaration 70 | class Vector3; 71 | class Quaternion; 72 | class SceneNode; 73 | class Entity; 74 | 75 | } // End namespace Ogre 76 | 77 | namespace rviz_mesh_plugin 78 | { 79 | 80 | /** 81 | * @brief Class to display mesh data in the main panel of rviz. 82 | */ 83 | class TriangleMeshVisual 84 | { 85 | public: 86 | 87 | /** 88 | * @brief Constructor. 89 | * 90 | * @param context The context that contains the display information. 91 | * @param displayID The display id 92 | * @param meshID The mesh id 93 | * @param randomID random number that will be used as part of the meshes UID 94 | */ 95 | TriangleMeshVisual(rviz::DisplayContext* context, 96 | size_t displayID, 97 | size_t meshID, 98 | size_t randomID); 99 | 100 | /** 101 | * @brief Destructor. 102 | */ 103 | virtual ~TriangleMeshVisual(); 104 | 105 | /** 106 | * @brief Clears whole stored data. 107 | */ 108 | void reset(); 109 | 110 | /** 111 | * @brief Extracts data from the ros-messages and creates meshes. 112 | * 113 | * @param meshMsg Message containing the mesh 114 | */ 115 | void setMessage(const mesh_msgs::TriangleMeshStamped::ConstPtr& meshMsg); 116 | 117 | 118 | /** 119 | * @brief Sets the pose of the coordinate frame the message refers to. 120 | * 121 | * @param position The pose of the coordinate frame 122 | */ 123 | void setFramePosition(const Ogre::Vector3& position); 124 | 125 | /** 126 | * @brief Sets the orientation of the coordinate frame the message refers to. 127 | * 128 | * @param orientation The orientation of the coordinate frame 129 | */ 130 | void setFrameOrientation(const Ogre::Quaternion& orientation); 131 | 132 | /** 133 | * @brief Updates the visible parts of the mesh depending on input from the rviz display. 134 | * 135 | * @param showWireframe When TRUE wireframe is visible 136 | * @param wireframeColor The color of the wireframe 137 | * @param wireframeAlpha The transparency of the wireframe 138 | * @param showFaces When TRUE faces are visible 139 | * @param facesColor The color of the faces 140 | * @param facesAlpha The transparency of the faces 141 | * @param useVertexColors When TRUE vertex colors are used 142 | * @param useTriangleColors When TRUE triangle colors are used 143 | * @param showTextures When TRUE textures are visible 144 | * @param showNormals When TRUE normals are visible 145 | * @param normalsColor The color of the normals 146 | * @param normalsAlpha The transparency of the normals 147 | * @param normalsScallingFactor The size of the normals 148 | */ 149 | void updateMaterial(bool showWireframe, 150 | Ogre::ColourValue wireframeColor, 151 | float wireframeAlpha, 152 | bool showFaces, 153 | Ogre::ColourValue facesColor, 154 | float facesAlpha, 155 | bool useVertexColors, 156 | bool useTriangleColors, 157 | bool showTextures, 158 | bool showNormals, 159 | Ogre::ColourValue normalsColor, 160 | float normalsAlpha, 161 | float normalsScallingFactor); 162 | 163 | /** 164 | * @brief Updates the size of the normals dynamically. 165 | * 166 | * @param scallingFactor The factor the normals have to be scaled with 167 | */ 168 | void updateNormals(float scallingFactor); 169 | 170 | private: 171 | 172 | void showWireframe( 173 | Ogre::Pass* pass, 174 | Ogre::ColourValue wireframeColor, 175 | float wireframeAlpha); 176 | 177 | void showFaces( 178 | Ogre::Pass* pass, 179 | Ogre::ColourValue facesColor, 180 | float facesAlpha, 181 | bool useVertexColors); 182 | 183 | void showNormals( 184 | Ogre::Pass* pass, 185 | Ogre::ColourValue normalsColor, 186 | float normalsAlpha); 187 | 188 | void showTextures(Ogre::Pass* pass); 189 | 190 | void enteringGeneralTriangleMesh(const mesh_msgs::TriangleMesh& mesh); 191 | void enteringColoredTriangleMesh(const mesh_msgs::TriangleMesh& mesh); 192 | void enteringNormals(const mesh_msgs::TriangleMesh& mesh); 193 | 194 | bool m_vertex_normals_enabled; 195 | bool m_vertex_colors_enabled; 196 | bool m_triangle_colors_enabled; 197 | bool m_texture_coords_enabled; 198 | 199 | //! Ogre Scenenode 200 | Ogre::SceneNode* m_sceneNode; 201 | 202 | //! The context that contains the display information. 203 | rviz::DisplayContext* m_displayContext; 204 | 205 | //! First ID of the created mesh 206 | size_t m_prefix; 207 | 208 | //! Second ID of the created mesh 209 | size_t m_postfix; 210 | 211 | //! Random ID of the created mesh 212 | size_t m_random; 213 | 214 | //! The mesh-object to display 215 | Ogre::ManualObject* m_mesh; 216 | 217 | //! The material for the general mesh 218 | Ogre::MaterialPtr m_meshGeneralMaterial; 219 | 220 | //! The material for the colored triangle mesh 221 | Ogre::MaterialPtr m_meshColoredTrianglesMaterial; 222 | 223 | //! The material of the normals 224 | Ogre::MaterialPtr m_normalMaterial; 225 | 226 | //! Counter for textures 227 | size_t m_textureCounter; 228 | 229 | //! Is the mesh textured? 230 | bool m_hasTextures; 231 | 232 | //! Map of fake textures (size = 1x1 = only a color) 233 | std::map m_fakeTextures; 234 | 235 | //! Factor the normal-size is multiplied with. 236 | float m_normalsScalingFactor; 237 | }; 238 | } // End namespace rviz_mesh_plugin 239 | 240 | #endif 241 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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 | #pragma once 49 | 50 | // enable exceptions for OpenCL 51 | #define CL_HPP_TARGET_OPENCL_VERSION 120 52 | #define CL_HPP_MINIMUM_OPENCL_VERSION 110 53 | #define CL_HPP_ENABLE_EXCEPTIONS 54 | 55 | #include 56 | 57 | #include 58 | 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | 65 | #include 66 | #include 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 | 81 | #include 82 | #include 83 | #include 84 | 85 | #include 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 | 99 | #endif 100 | 101 | namespace rviz{ 102 | class RosTopicProperty; 103 | class ColorProperty; 104 | } 105 | 106 | // OGRE stuff 107 | namespace Ogre 108 | { 109 | // Forward declaration 110 | class Vector3; 111 | } 112 | 113 | namespace rviz_map_plugin 114 | { 115 | // Forward declarations 116 | class ClusterLabelDisplay; 117 | class ClusterLabelVisual; 118 | 119 | /** 120 | * @class ClusterLabelTool 121 | * @brief Tool for selecting faces 122 | */ 123 | class ClusterLabelTool: public rviz::Tool 124 | { 125 | Q_OBJECT 126 | public: 127 | 128 | /** 129 | * @brief Constructor 130 | */ 131 | ClusterLabelTool(); 132 | 133 | /** 134 | * @brief Destructor 135 | */ 136 | ~ClusterLabelTool(); 137 | 138 | /** 139 | * @brief RViz callback on initialize 140 | */ 141 | virtual void onInitialize(); 142 | 143 | /** 144 | * @brief RViz callback for activating 145 | */ 146 | virtual void activate(); 147 | 148 | /** 149 | * @bríef RViz callback for deactivating 150 | */ 151 | virtual void deactivate(); 152 | 153 | /** 154 | * @brief RViz callback for mouse events 155 | * @param event The mouse event 156 | * @return Exit code 157 | */ 158 | virtual int processMouseEvent(rviz::ViewportMouseEvent& event); 159 | 160 | /** 161 | * @brief Connects this tool with a given display 162 | * @param display The display that creates this tool 163 | */ 164 | void setDisplay(ClusterLabelDisplay* display); 165 | 166 | /** 167 | * @brief Connects this tool with a given visual 168 | * @param visual The visual that will become editable with this tool 169 | */ 170 | void setVisual(std::shared_ptr visual); 171 | 172 | /** 173 | * @brief Adjust the sphere size for the brush tool 174 | * @param size The sphere size 175 | */ 176 | void setSphereSize(float size); 177 | 178 | public Q_SLOTS: 179 | 180 | /** 181 | * @brief Publish a label with a given namen 182 | * @param name The label name 183 | */ 184 | void publishLabel(std::string name); 185 | 186 | /** 187 | * @brief Returns a list of selected face ID's 188 | * @return List of face ID's 189 | */ 190 | std::vector getSelectedFaces(); 191 | 192 | /** 193 | * @brief Resets the list of selected faces 194 | */ 195 | void resetFaces(); 196 | 197 | /** 198 | * @brief Resets the current visual 199 | */ 200 | void resetVisual(); 201 | 202 | private: 203 | 204 | std::vector m_selectedFaces; 205 | std::vector m_faceSelectedArray; 206 | bool m_displayInitialized; 207 | ClusterLabelDisplay* m_display; 208 | std::shared_ptr m_visual; 209 | std::shared_ptr m_meshGeometry; 210 | float m_sphereSize = 1.0f; 211 | 212 | // Selection Box 213 | rviz::DisplayContext* m_displayContext; 214 | Ogre::SceneNode* m_sceneNode; 215 | Ogre::ManualObject* m_selectionBox; 216 | Ogre::MaterialPtr m_selectionBoxMaterial; 217 | Ogre::Vector2 m_selectionStart; 218 | Ogre::Vector2 m_selectionStop; 219 | bool m_multipleSelect = false; 220 | bool m_singleSelect = false; 221 | bool m_singleDeselect = false; 222 | 223 | std::vector m_vertexPositions; 224 | 225 | void updateSelectionBox(); 226 | void selectionBoxStart(rviz::ViewportMouseEvent& event); 227 | void selectionBoxMove(rviz::ViewportMouseEvent& event); 228 | void selectMultipleFaces(rviz::ViewportMouseEvent& event, bool selectMode); 229 | void selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode); 230 | void selectSingleFace(rviz::ViewportMouseEvent& event, bool selectMode); 231 | void selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode); 232 | void selectSphereFaces(rviz::ViewportMouseEvent& event, bool selectMode); 233 | void selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode); 234 | boost::optional> getClosestIntersectedFaceParallel(Ogre::Ray& ray); 235 | 236 | ros::Publisher m_labelPublisher; 237 | 238 | std::vector m_vertexData; 239 | std::array m_rayData; 240 | std::array m_sphereData; 241 | std::array m_startNormalData; 242 | std::vector m_boxData; 243 | std::vector m_resultDistances; 244 | 245 | // OpenCL 246 | cl::Device m_clDevice; 247 | cl::Context m_clContext; 248 | cl::Program::Sources m_clProgramSources; 249 | cl::Program m_clProgram; 250 | cl::CommandQueue m_clQueue; 251 | cl::Buffer m_clVertexBuffer; 252 | cl::Buffer m_clResultBuffer; 253 | cl::Buffer m_clRayBuffer; 254 | cl::Buffer m_clSphereBuffer; 255 | cl::Buffer m_clBoxBuffer; 256 | cl::Buffer m_clStartNormalBuffer; 257 | cl::Kernel m_clKernelSingleRay; 258 | cl::Kernel m_clKernelSphere; 259 | cl::Kernel m_clKernelBox; 260 | cl::Kernel m_clKernelDirAndDist; 261 | 262 | }; 263 | } // end namespace rviz_map_plugin 264 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/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 | 50 | #ifndef CLUSTER_LABEL_DISPLAY_HPP 51 | #define CLUSTER_LABEL_DISPLAY_HPP 52 | 53 | #include 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | 67 | #include 68 | #include 69 | #include 70 | 71 | #include 72 | #include 73 | #include 74 | #include 75 | #include 76 | #include 77 | 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 | #include 94 | #include 95 | 96 | #ifndef Q_MOC_RUN 97 | #include 98 | 99 | #include 100 | #include 101 | #include 102 | #include 103 | #include 104 | #include 105 | #include 106 | #include 107 | #include 108 | 109 | #endif 110 | 111 | namespace rviz 112 | { 113 | 114 | // Forward declaration 115 | class BoolProperty; 116 | class ColorProperty; 117 | class FloatProperty; 118 | class IntProperty; 119 | class EnumProperty; 120 | class StringProperty; 121 | 122 | } // End namespace rviz 123 | 124 | namespace rviz_map_plugin 125 | { 126 | 127 | using std::shared_ptr; 128 | using std::unique_ptr; 129 | using std::string; 130 | using std::vector; 131 | using std::map; 132 | 133 | // Forward declaration 134 | class ClusterLabelVisual; 135 | class ClusterLabelTool; 136 | 137 | 138 | /** 139 | * @class ClusterLabelDisplay 140 | * @brief Display class for the map plugin 141 | */ 142 | class ClusterLabelDisplay: public rviz::Display 143 | { 144 | 145 | Q_OBJECT 146 | 147 | public: 148 | 149 | /** 150 | * @brief Constructor 151 | */ 152 | ClusterLabelDisplay(); 153 | 154 | /** 155 | * @brief Destructor 156 | */ 157 | ~ClusterLabelDisplay(); 158 | 159 | /** 160 | * @brief The tool will call this function and emit the signal below to the master display to 161 | * create the label 162 | * @param label The label name 163 | * @param faces The list of face IDs 164 | */ 165 | void addLabel(string label, vector faces); 166 | 167 | /** 168 | * @brief RViz callback on enable 169 | */ 170 | void onEnable(); 171 | 172 | /** 173 | * @brief RViz callback on disable 174 | */ 175 | void onDisable(); 176 | 177 | Q_SIGNALS: 178 | 179 | /** 180 | * @brief This signal is used for delegating new label data to the master display. 181 | * @param cluster The cluster 182 | */ 183 | void signalAddLabel(Cluster cluster); 184 | 185 | public Q_SLOTS: // not sure wether any of those actually need to be q slots ... 186 | 187 | /** 188 | * @brief Refreshes the tool's current visual 189 | */ 190 | void notifyLabelTool(); 191 | 192 | /** 193 | * @brief Getter for the current geometry 194 | * @return The geometry 195 | */ 196 | shared_ptr getGeometry(); 197 | 198 | /** 199 | * @brief Setter for the geometry and cluster data 200 | * @param geometry The geometry 201 | * @param clusters The clusters 202 | */ 203 | void setData(shared_ptr geometry, vector clusters); 204 | 205 | private Q_SLOTS: 206 | 207 | /** 208 | * @brief Update the map, based on newly loaded data since the last update 209 | */ 210 | void updateMap(); 211 | 212 | /** 213 | * @brief Updates the colors, based on newly loaded data since the last update 214 | */ 215 | void updateColors(); 216 | 217 | /** 218 | * @brief Updates the sphere size for the brush tool 219 | */ 220 | void updateSphereSize(); 221 | 222 | /** 223 | * @brief Updates the phantom visual, based on newly loaded data since the last update 224 | */ 225 | void updatePhantomVisual(); 226 | 227 | /** 228 | * @brief Slot for changing the visual to the selected visual from the dropdown menu 229 | */ 230 | void changeVisual(); 231 | 232 | private: 233 | 234 | /** 235 | * @brief RViz callback on initialize 236 | */ 237 | void onInitialize(); 238 | 239 | /** 240 | * @brief Programmatically create an instance of the label tool from this package 241 | */ 242 | void initializeLabelTool(); 243 | 244 | /** 245 | * @brief Create visuals for each cluster in the list 246 | */ 247 | void createVisualsFromClusterList(); 248 | 249 | /** 250 | * @brief Creates a phantom visual 251 | */ 252 | void createPhantomVisual(); 253 | 254 | /** 255 | * @brief Dynamically fills the dropdown menus of those properties 256 | */ 257 | void fillPropertyOptions(); 258 | 259 | /// Geometry 260 | shared_ptr m_geometry; 261 | 262 | /// Visuals 263 | vector> m_visuals; 264 | 265 | /// ID of the current active visual 266 | uint32_t m_activeVisualId = 0; 267 | 268 | /// Additional visual to help with labeling without a TexturedMesh 269 | unique_ptr m_phantomVisual; 270 | 271 | /// Cluster data 272 | vector m_clusterList; 273 | 274 | /// Label tool 275 | ClusterLabelTool* m_tool; 276 | 277 | /// Property for the current active visual 278 | rviz::EnumProperty* m_activeVisualProperty; 279 | 280 | /// Property to set transparency 281 | rviz::FloatProperty* m_alphaProperty; 282 | 283 | /// Property for selecting colors (menu) 284 | rviz::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::FloatProperty* m_sphereSizeProperty; 291 | 292 | /// Property to hide or show a phantom visual 293 | rviz::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 | }; 302 | 303 | } // end namespace rviz_map_plugin 304 | 305 | #endif 306 | -------------------------------------------------------------------------------- /hdf5_map_io/include/hdf5_map_io/hdf5_map_io.h: -------------------------------------------------------------------------------- 1 | #ifndef HDF5_MAP_IO__H_ 2 | #define HDF5_MAP_IO__H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace hf = HighFive; 13 | 14 | namespace hdf5_map_io 15 | { 16 | 17 | /** 18 | * Helper struct to save vertices to the map. 19 | */ 20 | struct MapVertex { 21 | float x; 22 | float y; 23 | float z; 24 | }; 25 | 26 | 27 | /** 28 | * Helper struct to save textures / images to the map. 29 | */ 30 | struct MapImage { 31 | std::string name; 32 | uint32_t width; 33 | uint32_t height; 34 | uint32_t channels; 35 | std::vector data; 36 | }; 37 | 38 | /** 39 | * Helper struct for saving material data to the map. 40 | * 41 | * This struct is defined as an HDF compound data type. 42 | */ 43 | struct MapMaterial { 44 | int32_t textureIndex; 45 | uint8_t r; 46 | uint8_t g; 47 | uint8_t b; 48 | }; 49 | 50 | /** 51 | * This class if responsible for the map format. It tries to abstract most if not all calls to the 52 | * underlying HDF5 API and the HighFive wrapper. Furthermore it ensures the defined map format is always 53 | * in place and not tinkered with. 54 | * 55 | * NOTE: the map file is held open for the whole live time of this object. Thus it is possible that some data is 56 | * only written to disc if the destructor is called or the program has ended. Also make sure the map file is not opened 57 | * in any other way. (i.e. with the HDF5 Viewer). This will always lead to errors trying to access the file. 58 | */ 59 | class HDF5MapIO 60 | { 61 | public: 62 | /** 63 | * @brief Opens a map file for reading and writing. 64 | */ 65 | HDF5MapIO(std::string filename); 66 | 67 | /** 68 | * @brief Creates a map file (or truncates if the file already exists). 69 | */ 70 | HDF5MapIO( 71 | std::string filename, 72 | const std::vector& vertices, 73 | const std::vector& face_ids 74 | ); 75 | 76 | /** 77 | * @brief Closes main groups and makes sure all buffers are flushed to the file on disc. 78 | */ 79 | ~HDF5MapIO(); 80 | 81 | /** 82 | * @brief Returns vertices vector 83 | */ 84 | std::vector getVertices(); 85 | 86 | /** 87 | * @brief Returns face ids vector 88 | */ 89 | std::vector getFaceIds(); 90 | 91 | /** 92 | * @brief Returns vertex normals vector 93 | */ 94 | std::vector getVertexNormals(); 95 | 96 | /** 97 | * @brief Returns vertex colors vector 98 | */ 99 | std::vector getVertexColors(); 100 | 101 | /** 102 | * @brief Returns textures vector 103 | */ 104 | std::vector getTextures(); 105 | 106 | /** 107 | * @brief Returns an map which keys are representing the features point in space and the values 108 | * are an vector of floats representing the keypoints. 109 | */ 110 | std::unordered_map> getFeatures(); 111 | 112 | /** 113 | * @brief Returns materials as MapMaterial 114 | */ 115 | std::vector getMaterials(); 116 | 117 | /** 118 | * @brief Returns material <-> face indices 119 | */ 120 | std::vector getMaterialFaceIndices(); 121 | 122 | /** 123 | * @brief Returns vertex texture coordinates 124 | */ 125 | std::vector getVertexTextureCoords(); 126 | 127 | /** 128 | * @brief Returns all available label groups 129 | */ 130 | std::vector getLabelGroups(); 131 | 132 | /** 133 | * @brief Returns all labels inside the given group 134 | */ 135 | std::vector getAllLabelsOfGroup(std::string groupName); 136 | 137 | /** 138 | * @brief Returns face ids for the given label inside the group. 139 | * E.g: label=tree_1 -> groupName=tree; labelName=1 140 | */ 141 | std::vector getFaceIdsOfLabel(std::string groupName, std::string labelName); 142 | 143 | /** 144 | * @brief Returns the roughness as float vector. 145 | */ 146 | std::vector getRoughness(); 147 | 148 | /** 149 | * @brief Returns the height difference as float vector. 150 | */ 151 | std::vector getHeightDifference(); 152 | 153 | /** 154 | * @brief Returns the image in the group, if it exists. If not an empty struct is returned 155 | */ 156 | MapImage getImage(hf::Group group, std::string name); 157 | 158 | /** 159 | * @brief Add normals to the attributes group. 160 | */ 161 | hf::DataSet addVertexNormals(std::vector& normals); 162 | 163 | /** 164 | * @brief Add vertex colors to the attributes group. 165 | */ 166 | hf::DataSet addVertexColors(std::vector& colors); 167 | 168 | /** 169 | * Add texture img with given index to the textures group. Texture CAN NOT be overridden 170 | */ 171 | void addTexture(int index, uint32_t width, uint32_t height, uint8_t* data); 172 | 173 | /** 174 | * @brief Add materials as MapMaterial and the corresponding material <-> face indices 175 | */ 176 | void addMaterials(std::vector& materials, std::vector& matFaceIndices); 177 | 178 | /** 179 | * @brief Add vertex texture coordinates to the textures group. 180 | */ 181 | void addVertexTextureCoords(std::vector& coords); 182 | 183 | /** 184 | * @brief Adds the label (labelName) to the label group with the given faces. 185 | * E.g.: tree_1 -> groupName=tree; labelName=1; separated by the '_' 186 | */ 187 | void addLabel(std::string groupName, std::string labelName, std::vector& faceIds); 188 | 189 | /** 190 | * @brief Adds the keypoints with their corresponding positions to the attributes_group. The position 191 | * is saved to the entry via an attribute called 'vector'. 192 | */ 193 | void addTextureKeypointsMap(std::unordered_map>& keypoints_map); 194 | 195 | /** 196 | * @brief Adds the roughness to the attributes group. 197 | */ 198 | void addRoughness(std::vector& roughness); 199 | 200 | /** 201 | * @brief Adds the height difference to the attributes group. 202 | */ 203 | void addHeightDifference(std::vector& diff); 204 | 205 | /** 206 | * @brief Adds an image with given data set name to the given group 207 | */ 208 | void addImage(hf::Group group, 209 | std::string name, 210 | const uint32_t width, 211 | const uint32_t height, 212 | const uint8_t* pixelBuffer 213 | ); 214 | 215 | /** 216 | * Removes all labels from the file. 217 | *
218 | * Be careful, this does not clear up the space of the labels. Use the cli tool 'h5repack' manually to clear up 219 | * all wasted space if this method was used multiple times. 220 | * 221 | * @return true if removing all labels successfully. 222 | */ 223 | bool removeAllLabels(); 224 | 225 | /** 226 | * @brief Flushes the file. All opened buffers are saved to disc. 227 | */ 228 | void flush(); 229 | 230 | private: 231 | hf::File m_file; 232 | 233 | // group names 234 | 235 | static constexpr const char* GEOMETRY_GROUP = "/geometry"; 236 | static constexpr const char* ATTRIBUTES_GROUP = "/attributes"; 237 | static constexpr const char* CLUSTERSETS_GROUP = "/clustersets"; 238 | static constexpr const char* TEXTURES_GROUP = "/textures"; 239 | static constexpr const char* LABELS_GROUP = "/labels"; 240 | 241 | // main groups for reference 242 | hf::Group m_geometryGroup; 243 | hf::Group m_attributesGroup; 244 | hf::Group m_clusterSetsGroup; 245 | hf::Group m_texturesGroup; 246 | hf::Group m_labelsGroup; 247 | }; 248 | } // namespace hdf5_map_io 249 | 250 | 251 | namespace HighFive { 252 | 253 | /** 254 | * Define the MapMaterial as an HDF5 compound data type. 255 | */ 256 | template <> 257 | inline AtomicType::AtomicType() 258 | { 259 | hid_t materialHid = H5Tcreate(H5T_COMPOUND, sizeof(hdf5_map_io::MapMaterial)); 260 | 261 | H5Tinsert(materialHid, "textureIndex", offsetof(hdf5_map_io::MapMaterial, textureIndex), H5T_NATIVE_INT); 262 | H5Tinsert(materialHid, "r", offsetof(hdf5_map_io::MapMaterial, r), H5T_NATIVE_UCHAR); 263 | H5Tinsert(materialHid, "g", offsetof(hdf5_map_io::MapMaterial, g), H5T_NATIVE_UCHAR); 264 | H5Tinsert(materialHid, "b", offsetof(hdf5_map_io::MapMaterial, b), H5T_NATIVE_UCHAR); 265 | 266 | _hid = H5Tcopy(materialHid); 267 | } 268 | 269 | } 270 | 271 | namespace std 272 | { 273 | template <> 274 | struct hash 275 | { 276 | size_t operator()(const hdf5_map_io::MapVertex& k) const 277 | { 278 | size_t h1 = std::hash()(k.x); 279 | size_t h2 = std::hash()(k.y); 280 | size_t h3 = std::hash()(k.z); 281 | return (h1 ^ (h2 << 1)) ^ h3; 282 | } 283 | }; 284 | 285 | template <> 286 | struct equal_to 287 | { 288 | bool operator()( const hdf5_map_io::MapVertex& lhs, const hdf5_map_io::MapVertex& rhs ) const{ 289 | return (lhs.x == rhs.x) && (lhs.y == rhs.y) && (lhs.z == rhs.z); 290 | } 291 | }; 292 | 293 | } 294 | 295 | #endif // HDF5_MAP_IO__H_ 296 | -------------------------------------------------------------------------------- /rviz_mesh_plugin/src/mesh_pose_tool.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_pose_tool.cpp 41 | * 42 | * authors: Sebastian Pütz 43 | * 44 | */ 45 | 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | #include "mesh_pose_tool.h" 61 | 62 | namespace rviz_mesh_plugin 63 | { 64 | 65 | MeshPoseTool::MeshPoseTool() 66 | : rviz::Tool(), 67 | arrow_( NULL ) 68 | { 69 | } 70 | 71 | MeshPoseTool::~MeshPoseTool() 72 | { 73 | delete arrow_; 74 | } 75 | 76 | void MeshPoseTool::onInitialize() 77 | { 78 | arrow_ = new rviz::Arrow( scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f ); 79 | arrow_->setColor( 0.0f, 1.0f, 0.0f, 1.0f ); 80 | arrow_->getSceneNode()->setVisible( false ); 81 | } 82 | 83 | void MeshPoseTool::activate() 84 | { 85 | setStatus( "Click and on a mesh_msgs::TriangleMesh to set the position and drag the mouse for the orientation." ); 86 | state_ = Position; 87 | } 88 | 89 | void MeshPoseTool::deactivate() 90 | { 91 | arrow_->getSceneNode()->setVisible( false ); 92 | } 93 | 94 | int MeshPoseTool::processMouseEvent( rviz::ViewportMouseEvent& event ) 95 | { 96 | int flags = 0; 97 | 98 | if( event.leftDown() ) 99 | { 100 | ROS_ASSERT( state_ == Position ); 101 | 102 | Ogre::Vector3 pos, ori; 103 | if(selectTriangle(event, pos, ori)){ 104 | pos_ = pos; 105 | ori_ = ori; 106 | arrow_->setPosition( pos_ ); 107 | state_ = Orientation; 108 | flags |= Render; 109 | } 110 | } 111 | else if( event.type == QEvent::MouseMove && event.left() ) 112 | { 113 | if( state_ == Orientation ) 114 | { 115 | Ogre::Vector3 cur_pos; 116 | Ogre::Plane plane( ori_, pos_ ); 117 | if( rviz::getPointOnPlaneFromWindowXY( event.viewport, 118 | plane, event.x, event.y, cur_pos )) 119 | { 120 | // right hand coordinate system 121 | // x to the right, y to the top, and -z into the scene 122 | // arrow foreward negative z 123 | Ogre::Vector3 z_axis = - (cur_pos - pos_); 124 | Ogre::Vector3 y_axis = ori_; 125 | Ogre::Vector3 x_axis = y_axis.crossProduct( z_axis ); 126 | 127 | x_axis.normalise(); 128 | y_axis.normalise(); 129 | z_axis.normalise(); 130 | 131 | arrow_->getSceneNode()->setVisible( true ); 132 | arrow_->setOrientation( Ogre::Quaternion( x_axis, y_axis, z_axis) ); 133 | 134 | flags |= Render; 135 | } 136 | } 137 | } 138 | else if( event.leftUp() ) 139 | { 140 | if( state_ == Orientation ) 141 | { 142 | Ogre::Vector3 cur_pos; 143 | Ogre::Plane plane( ori_, pos_ ); 144 | if( rviz::getPointOnPlaneFromWindowXY( event.viewport, 145 | plane, event.x, event.y, cur_pos )) 146 | { 147 | // arrow foreward negative z 148 | Ogre::Vector3 z_axis = - (cur_pos - pos_); 149 | Ogre::Vector3 y_axis = ori_; 150 | Ogre::Vector3 x_axis = y_axis.crossProduct( z_axis ); 151 | 152 | x_axis.normalise(); 153 | y_axis.normalise(); 154 | z_axis.normalise(); 155 | 156 | onPoseSet(pos_, Ogre::Quaternion(x_axis, y_axis, z_axis)); 157 | 158 | flags |= (Finished|Render); 159 | } 160 | } 161 | } 162 | 163 | return flags; 164 | } 165 | 166 | bool MeshPoseTool::selectTriangle(rviz::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& triangle_normal) 167 | { 168 | Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay 169 | ((float) event.x / event.viewport->getActualWidth(), 170 | (float) event.y / event.viewport->getActualHeight()); 171 | 172 | Ogre::RaySceneQuery* query = context_->getSceneManager()->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); 173 | query->setSortByDistance(true); 174 | 175 | Ogre::RaySceneQueryResult& result = query->execute(); 176 | 177 | for (size_t i=0; igetName().find("TriangleMesh") != std::string::npos){ 180 | Ogre::ManualObject* mesh = static_cast(result[i].movable); 181 | size_t goal_section = -1; 182 | size_t goal_index = -1; 183 | Ogre::Real dist = -1; 184 | if(getPositionAndOrientation(mesh, ray, position, triangle_normal)){ 185 | return true; 186 | } 187 | } 188 | } 189 | return false; 190 | } 191 | 192 | 193 | 194 | bool MeshPoseTool::getPositionAndOrientation( 195 | const Ogre::ManualObject* mesh, 196 | const Ogre::Ray &ray, 197 | Ogre::Vector3& position, 198 | Ogre::Vector3& orientation) 199 | { 200 | Ogre::Real dist = -1.0f; 201 | Ogre::Vector3 a, b, c; 202 | 203 | size_t vertex_count = 0; 204 | Ogre::Vector3* vertices; 205 | size_t index_count = 0; 206 | unsigned long* indices; 207 | size_t num_sections = mesh->getNumSections(); 208 | 209 | for (size_t i = 0; i < num_sections; i++) 210 | { 211 | getRawManualObjectData(mesh, i, vertex_count, vertices, index_count, indices); 212 | if(index_count != 0) 213 | { 214 | for (size_t j = 0; j < index_count; j += 3) 215 | { 216 | std::pair goal = 217 | Ogre::Math::intersects( 218 | ray, 219 | vertices[indices[j]], 220 | vertices[indices[j + 1]], 221 | vertices[indices[j + 2]], 222 | true, 223 | true); 224 | 225 | if (goal.first) 226 | { 227 | if ((dist < 0.0f) || (goal.second < dist)) 228 | { 229 | dist = goal.second; 230 | a = vertices[indices[j]]; 231 | b = vertices[indices[j + 1]]; 232 | c = vertices[indices[j + 2]]; 233 | } 234 | } 235 | } 236 | } 237 | } 238 | 239 | delete[] vertices; 240 | delete[] indices; 241 | if(dist != -1){ 242 | position = ray.getPoint(dist); 243 | Ogre::Vector3 ab = b-a; 244 | Ogre::Vector3 ac = c-a; 245 | orientation = ac.crossProduct(ab).normalisedCopy(); 246 | return true; 247 | }else{ 248 | return false; 249 | } 250 | } 251 | 252 | void MeshPoseTool::getRawManualObjectData( 253 | const Ogre::ManualObject *mesh, 254 | const size_t sectionNumber, 255 | size_t& vertexCount, 256 | Ogre::Vector3*& vertices, 257 | size_t& indexCount, 258 | unsigned long*& indices 259 | ){ 260 | Ogre::VertexData* vertexData; 261 | const Ogre::VertexElement* vertexElement; 262 | Ogre::HardwareVertexBufferSharedPtr vertexBuffer; 263 | unsigned char* vertexChar; 264 | float* vertexFloat; 265 | 266 | vertexData = mesh->getSection(sectionNumber)->getRenderOperation()->vertexData; 267 | vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION); 268 | vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource()); 269 | vertexChar = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); 270 | 271 | vertexCount = vertexData->vertexCount; 272 | vertices = new Ogre::Vector3[vertexCount]; 273 | 274 | for (size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize()) 275 | { 276 | vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat); 277 | vertices[i] = (mesh->getParentNode()->_getDerivedOrientation() * 278 | (Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]) * mesh->getParentNode()->_getDerivedScale())) + 279 | mesh->getParentNode()->_getDerivedPosition(); 280 | } 281 | 282 | vertexBuffer->unlock(); 283 | 284 | Ogre::IndexData* indexData; 285 | Ogre::HardwareIndexBufferSharedPtr indexBuffer; 286 | indexData = mesh->getSection(sectionNumber)->getRenderOperation()->indexData; 287 | indexCount = indexData->indexCount; 288 | indices = new unsigned long[indexCount]; 289 | indexBuffer = indexData->indexBuffer; 290 | unsigned int* pLong = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); 291 | unsigned short* pShort = reinterpret_cast(pLong); 292 | 293 | for (size_t i = 0; i < indexCount; i++) 294 | { 295 | unsigned long index; 296 | if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT) 297 | { 298 | index = static_cast(pLong[i]); 299 | } 300 | 301 | else 302 | { 303 | index = static_cast(pShort[i]); 304 | } 305 | 306 | indices[i] = index; 307 | } 308 | indexBuffer->unlock(); 309 | } 310 | 311 | } /* rviz_mesh_plugin */ 312 | 313 | -------------------------------------------------------------------------------- /label_manager/src/manager.cpp: -------------------------------------------------------------------------------- 1 | #include "label_manager/manager.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "mesh_msgs/MeshFaceCluster.h" 8 | 9 | using namespace boost::filesystem; 10 | 11 | namespace label_manager 12 | { 13 | LabelManager::LabelManager(ros::NodeHandle& nodeHandle) : 14 | nh(nodeHandle) { 15 | 16 | if (!nh.getParam("folder_path", folderPath)) 17 | { 18 | folderPath = "/tmp/label_manager/"; 19 | } 20 | 21 | path p(folderPath); 22 | if (!is_directory(p) && !exists(p)) 23 | { 24 | create_directory(p); 25 | } 26 | 27 | clusterLabelSub = nh.subscribe("cluster_label", 10, &LabelManager::clusterLabelCallback, this); 28 | newClusterLabelPub = nh.advertise("new_cluster_label", 1); 29 | srv_get_labeled_clusters = nh.advertiseService( 30 | "get_labeled_clusters", 31 | &LabelManager::service_getLabeledClusters, 32 | this 33 | ); 34 | srv_get_label_groups = nh.advertiseService( 35 | "get_label_groups", 36 | &LabelManager::service_getLabelGroups, 37 | this 38 | ); 39 | srv_get_labeled_cluster_group = nh.advertiseService( 40 | "get_labeled_cluster_group", 41 | &LabelManager::service_getLabeledClusterGroup, 42 | this 43 | ); 44 | srv_delete_label = nh.advertiseService( 45 | "delete_label", 46 | &LabelManager::service_deleteLabel, 47 | this 48 | ); 49 | 50 | ROS_INFO("Started LabelManager"); 51 | 52 | ros::spin(); 53 | } 54 | 55 | void LabelManager::clusterLabelCallback(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg) 56 | { 57 | ROS_INFO_STREAM("Got msg for mesh: " << msg->uuid << " with label: " << msg->cluster.label); 58 | 59 | std::vector indices; 60 | std::string fileName = getFileName(msg->uuid, msg->cluster.label); 61 | 62 | // if appending (not override), first figure what new indices we have to add 63 | if (!msg->override) 64 | { 65 | std::vector readIndices = readIndicesFromFile(fileName); 66 | 67 | // if read indices is empty no file was found or could not be read 68 | if (readIndices.empty()) 69 | { 70 | indices = msg->cluster.face_indices; 71 | } 72 | else 73 | { 74 | for (size_t i = 0; i < msg->cluster.face_indices.size(); i++) 75 | { 76 | uint idx = msg->cluster.face_indices[i]; 77 | 78 | // if msg index is not already in file, add it to indices vector 79 | if (std::find(readIndices.begin(), readIndices.end(), idx) == readIndices.end()) 80 | { 81 | indices.push_back(idx); 82 | } 83 | } 84 | } 85 | } 86 | else 87 | { 88 | indices = msg->cluster.face_indices; 89 | } 90 | 91 | // publish every new labeled cluster 92 | newClusterLabelPub.publish(msg->cluster); 93 | 94 | // make sure mesh folder exists before writing 95 | path p(folderPath + "/" + msg->uuid); 96 | if (!is_directory(p) || !exists(p)) 97 | { 98 | create_directory(p); 99 | } 100 | 101 | writeIndicesToFile(fileName, indices, !msg->override); 102 | } 103 | 104 | bool LabelManager::service_getLabeledClusters( 105 | mesh_msgs::GetLabeledClusters::Request& req, 106 | mesh_msgs::GetLabeledClusters::Response& res 107 | ) 108 | { 109 | ROS_DEBUG_STREAM("Service call with uuid: " << req.uuid); 110 | 111 | path p (folderPath + "/" + req.uuid); 112 | directory_iterator end_itr; 113 | 114 | if (!is_directory(p) || !exists(p)) 115 | { 116 | ROS_DEBUG_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); 117 | 118 | return false; 119 | } 120 | 121 | for (directory_iterator itr(p); itr != end_itr; ++itr) 122 | { 123 | // if file is no dir 124 | if (is_regular_file(itr->path())) 125 | { 126 | std::string label = itr->path().filename().string(); 127 | // remove extension from label 128 | boost::replace_all(label, itr->path().filename().extension().string(), ""); 129 | 130 | mesh_msgs::MeshFaceCluster c; 131 | c.face_indices = readIndicesFromFile(itr->path().string()); 132 | c.label = label; 133 | 134 | res.clusters.push_back(c); 135 | } 136 | } 137 | 138 | return true; 139 | } 140 | 141 | bool LabelManager::service_getLabelGroups( 142 | label_manager::GetLabelGroups::Request& req, 143 | label_manager::GetLabelGroups::Response& res) 144 | { 145 | path p (folderPath + "/" + req.uuid); 146 | directory_iterator end_itr; 147 | 148 | if (!is_directory(p) || !exists(p)) 149 | { 150 | ROS_WARN_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); 151 | 152 | return false; 153 | } 154 | 155 | for (directory_iterator itr(p); itr != end_itr; ++itr) 156 | { 157 | // if file is no dir 158 | if (is_regular_file(itr->path())) 159 | { 160 | std::string label = itr->path().filename().string(); 161 | // remove extension from label 162 | boost::replace_all(label, itr->path().filename().extension().string(), ""); 163 | 164 | // assuming the labels will look like this: 'GROUP_SOMETHINGELSE', 165 | // remove everthing not representing the group 166 | // TODO make seperator configurable 167 | label = label.substr(0, label.find_first_of("_", 0)); 168 | 169 | // only add label group to response if not already added 170 | if (std::find(res.labels.begin(), res.labels.end(), label) == res.labels.end()) 171 | { 172 | res.labels.push_back(label); 173 | } 174 | } 175 | } 176 | 177 | 178 | return true; 179 | } 180 | 181 | bool LabelManager::service_deleteLabel( 182 | label_manager::DeleteLabel::Request& req, 183 | label_manager::DeleteLabel::Response& res) 184 | { 185 | path p(getFileName(req.uuid, req.label)); 186 | 187 | if (!is_regular_file(p) || !exists(p)) 188 | { 189 | ROS_WARN_STREAM("Could not delete label '" << req.label << "' of mesh '" << req.uuid << "'."); 190 | 191 | return false; 192 | } 193 | 194 | res.cluster.face_indices = readIndicesFromFile(p.filename().string()); 195 | res.cluster.label = req.label; 196 | 197 | return remove(p); 198 | } 199 | 200 | bool LabelManager::service_getLabeledClusterGroup( 201 | label_manager::GetLabeledClusterGroup::Request& req, 202 | label_manager::GetLabeledClusterGroup::Response& res) 203 | { 204 | path p (folderPath + "/" + req.uuid); 205 | directory_iterator end_itr; 206 | 207 | if (!is_directory(p) || !exists(p)) 208 | { 209 | ROS_WARN_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); 210 | 211 | return false; 212 | } 213 | 214 | for (directory_iterator itr(p); itr != end_itr; ++itr) 215 | { 216 | // if file is no dir 217 | if (is_regular_file(itr->path()) && itr->path().filename().string().find(req.labelGroup) == 0) 218 | { 219 | std::string label = itr->path().filename().string(); 220 | // remove extension from label 221 | boost::replace_all(label, itr->path().filename().extension().string(), ""); 222 | 223 | mesh_msgs::MeshFaceCluster c; 224 | c.face_indices = readIndicesFromFile(itr->path().string()); 225 | c.label = label; 226 | 227 | res.clusters.push_back(c); 228 | } 229 | } 230 | 231 | 232 | return true; 233 | } 234 | 235 | bool LabelManager::writeIndicesToFile( 236 | const std::string& fileName, 237 | const std::vector& indices, 238 | const bool append 239 | ) 240 | { 241 | if (indices.empty()) 242 | { 243 | ROS_WARN_STREAM("Empty indices."); 244 | 245 | return true; 246 | } 247 | 248 | std::ios_base::openmode mode = append ? (std::ios::out|std::ios::app) : std::ios::out; 249 | std::ofstream ofs(fileName.c_str(), mode); 250 | 251 | ROS_DEBUG_STREAM("Writing indices to file: " << fileName); 252 | 253 | if (ofs.is_open()) 254 | { 255 | // if in append mode add , after the old data 256 | if (append) 257 | { 258 | ofs << ","; 259 | } 260 | 261 | size_t size = indices.size(); 262 | for (size_t i = 0; i < size; i++) 263 | { 264 | ofs << indices[i]; 265 | 266 | if (i < size - 1) 267 | { 268 | ofs << ","; 269 | } 270 | } 271 | 272 | ofs.close(); 273 | ROS_DEBUG_STREAM("Successfully written indices to file."); 274 | 275 | return true; 276 | } 277 | else 278 | { 279 | ROS_ERROR_STREAM("Could not open file: " << fileName); 280 | } 281 | 282 | return false; 283 | } 284 | 285 | std::vector LabelManager::readIndicesFromFile(const std::string& fileName) 286 | { 287 | std::ifstream ifs(fileName.c_str(), std::ios::in); 288 | std::vector faceIndices; 289 | 290 | // if file dos not exists, return empty vector 291 | if (!ifs.good()) 292 | { 293 | ROS_DEBUG_STREAM("File " << fileName << " does not exists. Nothing to read..."); 294 | 295 | return faceIndices; 296 | } 297 | 298 | std::string stringNumber; 299 | while (std::getline(ifs, stringNumber, ',')) 300 | { 301 | faceIndices.push_back(atoi(stringNumber.c_str())); 302 | } 303 | 304 | return faceIndices; 305 | } 306 | 307 | std::string LabelManager::getFileName(const std::string& uuid, const std::string& label) 308 | { 309 | return folderPath + "/" +uuid + "/" + label + ".dat"; 310 | } 311 | } 312 | -------------------------------------------------------------------------------- /rviz_map_plugin/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 | 60 | namespace rviz_map_plugin 61 | { 62 | 63 | ClusterLabelVisual::ClusterLabelVisual( 64 | rviz::DisplayContext* context, 65 | std::string labelId 66 | ) : 67 | m_displayContext(context), 68 | m_labelId(labelId) 69 | { } 70 | 71 | ClusterLabelVisual::ClusterLabelVisual( 72 | rviz::DisplayContext* context, 73 | std::string labelId, 74 | std::shared_ptr geometry 75 | ) : 76 | m_displayContext(context), 77 | m_labelId(labelId), 78 | m_geometry(geometry) 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(). 117 | createVertexBuffer(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 | entity->setMaterialName("CustomMaterial", "General"); 146 | m_sceneNode->attachObject(entity); 147 | } 148 | 149 | // Create a submesh and a custom material for it 150 | if (!m_mesh.isNull()) 151 | { 152 | m_subMesh = m_mesh->createSubMesh(m_labelId); 153 | m_subMesh->useSharedVertices = true; 154 | std::stringstream sstm; 155 | sstm << "ClusterLabel_Material_" << m_labelId; 156 | m_material = 157 | Ogre::MaterialManager::getSingleton().create( 158 | sstm.str(), 159 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 160 | true 161 | ); 162 | 163 | m_subMesh->setMaterialName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 164 | m_material->getTechnique(0)->removeAllPasses(); 165 | m_material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 166 | m_material->setSceneBlending(Ogre::SBT_ADD); 167 | m_material->setDepthWriteEnabled(false); 168 | 169 | initMaterial(); 170 | } 171 | } 172 | 173 | ClusterLabelVisual::~ClusterLabelVisual() 174 | { 175 | reset(); 176 | 177 | if (!m_mesh.isNull()) 178 | { 179 | ROS_DEBUG("ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); 180 | 181 | try 182 | { 183 | m_mesh->destroySubMesh(m_labelId); 184 | } 185 | catch (...) 186 | { 187 | ROS_ERROR("Exception in Visual destructor"); 188 | } 189 | } 190 | 191 | if (m_sceneNode->numAttachedObjects() == 0) 192 | { 193 | ROS_INFO("ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); 194 | m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); 195 | } 196 | } 197 | 198 | void ClusterLabelVisual::reset() 199 | { 200 | if (!m_material.isNull()) 201 | { 202 | Ogre::MaterialManager::getSingleton().unload(m_material->getName()); 203 | Ogre::MaterialManager::getSingleton().remove(m_material->getName()); 204 | } 205 | } 206 | 207 | void ClusterLabelVisual::setGeometry(std::shared_ptr geometry) 208 | {} 209 | 210 | void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) 211 | { 212 | m_faces = faces; 213 | 214 | if (!m_geometry) 215 | { 216 | ROS_WARN("ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); 217 | return; 218 | } 219 | 220 | // don't draw the cluster if there are no faces in it 221 | if (faces.empty()) 222 | { 223 | m_subMesh->indexData->indexBuffer.setNull(); 224 | m_subMesh->indexData->indexCount = 0; 225 | m_subMesh->indexData->indexStart = 0; 226 | m_material->getTechnique(0)->removeAllPasses(); 227 | ROS_DEBUG("ClusterLabelVisual::setFacesInCluster: faces empty!"); 228 | return; 229 | } 230 | 231 | // if there are faces and there are no passes, create a pass to draw the cluster 232 | if (m_material->getTechnique(0)->getNumPasses() == 0) 233 | { 234 | m_material->getTechnique(0)->createPass(); 235 | m_material->setDiffuse(m_color); 236 | m_material->setSelfIllumination(m_color); 237 | 238 | initMaterial(); 239 | } 240 | 241 | size_t indexCount = faces.size() * 3; 242 | 243 | // Create the index buffer 244 | Ogre::HardwareIndexBufferSharedPtr indexBuffer = Ogre::HardwareBufferManager::getSingleton(). 245 | createIndexBuffer(Ogre::HardwareIndexBuffer::IT_32BIT, indexCount, Ogre::HardwareBuffer::HBU_WRITE_ONLY); 246 | 247 | // Lock the buffer so we can get exclusive access to its data 248 | uint32_t *indices = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_WRITE_ONLY)); 249 | 250 | // Define the triangles 251 | for (int i = 0; i < faces.size(); i++) 252 | { 253 | uint32_t faceId = faces[i]; 254 | indices[i * 3 + 0] = m_geometry->faces[faceId].vertexIndices[0]; 255 | indices[i * 3 + 1] = m_geometry->faces[faceId].vertexIndices[1]; 256 | indices[i * 3 + 2] = m_geometry->faces[faceId].vertexIndices[2]; 257 | } 258 | 259 | // Unlock the buffer 260 | indexBuffer->unlock(); 261 | 262 | // Attach the index buffer to the submesh 263 | m_subMesh->indexData->indexBuffer = indexBuffer; 264 | m_subMesh->indexData->indexCount = indexCount; 265 | m_subMesh->indexData->indexStart = 0; 266 | } 267 | 268 | void ClusterLabelVisual::setColor(Ogre::ColourValue facesColor, float alpha) 269 | { 270 | if (!m_material.isNull()) 271 | { 272 | facesColor.a = alpha; 273 | m_material->setDiffuse(facesColor); 274 | m_material->setSelfIllumination(facesColor); 275 | m_color = facesColor; 276 | } 277 | } 278 | 279 | void ClusterLabelVisual::initMaterial() 280 | { 281 | m_material->setCullingMode(Ogre::CULL_NONE); 282 | m_material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 283 | m_material->setDepthWriteEnabled(false); 284 | 285 | } 286 | 287 | } // End namespace rviz_map_plugin 288 | -------------------------------------------------------------------------------- /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 | 46 | 47 | #include "mesh_msgs_transform/transforms.h" 48 | #include 49 | 50 | namespace mesh_msgs_transform{ 51 | 52 | inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec){ 53 | eigen_vec(0) = tf_vec[0]; 54 | eigen_vec(1) = tf_vec[1]; 55 | eigen_vec(2) = tf_vec[2]; 56 | } 57 | 58 | inline void pointMsgToEigen(const geometry_msgs::Point& gm_point, Eigen::Vector3d& eigen_point){ 59 | eigen_point(0) = gm_point.x; 60 | eigen_point(1) = gm_point.y; 61 | eigen_point(2) = gm_point.z; 62 | } 63 | 64 | inline void pointEigenToMsg(const Eigen::Vector3d& eigen_point, geometry_msgs::Point& gm_point){ 65 | gm_point.x = eigen_point(0); 66 | gm_point.y = eigen_point(1); 67 | gm_point.z = eigen_point(2); 68 | } 69 | 70 | bool transformTriangleMeshNoTime( 71 | const std::string& target_frame, 72 | const mesh_msgs::TriangleMeshStamped& mesh_in, 73 | const std::string& fixed_frame, 74 | mesh_msgs::TriangleMeshStamped& mesh_out, 75 | const tf::TransformListener& tf_listener 76 | ) 77 | { 78 | tf::StampedTransform transform; 79 | try{ 80 | tf_listener.lookupTransform ( 81 | target_frame, 82 | ros::Time(0), mesh_in.header.frame_id, 83 | ros::Time(0), 84 | fixed_frame, 85 | transform 86 | ); 87 | } 88 | catch (tf::LookupException &e){ 89 | ROS_ERROR ("%s", e.what ()); 90 | return false; 91 | } 92 | catch (tf::ExtrapolationException &e){ 93 | ROS_ERROR ("%s", e.what ()); 94 | return false; 95 | } 96 | 97 | tf::Quaternion quaternion = transform.getRotation (); 98 | Eigen::Quaterniond rotation ( 99 | quaternion.w (), 100 | quaternion.x (), 101 | quaternion.y (), 102 | quaternion.z () 103 | ); 104 | 105 | tf::Vector3 origin = transform.getOrigin (); 106 | Eigen::Vector3d eigen_origin; 107 | vectorTfToEigen(origin, eigen_origin); 108 | Eigen::Translation3d translation ( eigen_origin ); 109 | Eigen::Affine3d transformation = translation * rotation; 110 | 111 | if (&mesh_in != &mesh_out){ 112 | mesh_out.header = mesh_in.header; 113 | mesh_out.header.stamp = ros::Time::now(); 114 | mesh_out.mesh.triangles = mesh_in.mesh.triangles; 115 | mesh_out.mesh.vertex_colors = mesh_in.mesh.vertex_colors; 116 | mesh_out.mesh.triangle_colors = mesh_in.mesh.triangle_colors; 117 | mesh_out.mesh.vertex_texture_coords = mesh_in.mesh.vertex_texture_coords; 118 | mesh_out.mesh.face_materials = mesh_in.mesh.face_materials; 119 | mesh_out.mesh.textures = mesh_in.mesh.textures; 120 | mesh_out.mesh.clusters = mesh_in.mesh.clusters; 121 | } 122 | 123 | mesh_out.mesh.vertices.resize(mesh_in.mesh.vertices.size()); 124 | mesh_out.mesh.vertex_normals.resize(mesh_in.mesh.vertex_normals.size()); 125 | 126 | // transform vertices 127 | for(size_t i = 0; i < mesh_in.mesh.vertices.size(); i++) 128 | { 129 | Eigen::Vector3d in_vec, out_vec; 130 | pointMsgToEigen(mesh_in.mesh.vertices[i], in_vec); 131 | out_vec = transformation * in_vec; 132 | pointEigenToMsg(out_vec, mesh_out.mesh.vertices[i]); 133 | } 134 | 135 | // rotate normals 136 | for(size_t i = 0; i < mesh_in.mesh.vertex_normals.size(); i++) 137 | { 138 | Eigen::Vector3d in_vec, out_vec; 139 | pointMsgToEigen(mesh_in.mesh.vertex_normals[i], in_vec); 140 | out_vec = transformation.rotation() * in_vec; 141 | pointEigenToMsg(out_vec, mesh_out.mesh.vertex_normals[i]); 142 | } 143 | 144 | mesh_out.header.frame_id = target_frame; 145 | mesh_out.header.stamp = ros::Time::now(); 146 | 147 | return true; 148 | } 149 | 150 | bool transformGeometryMeshNoTime( 151 | const std::string& target_frame, 152 | const mesh_msgs::MeshGeometryStamped& mesh_in, 153 | const std::string& fixed_frame, 154 | mesh_msgs::MeshGeometryStamped& mesh_out, 155 | const tf::TransformListener& tf_listener 156 | ) 157 | { 158 | tf::StampedTransform transform; 159 | try{ 160 | tf_listener.lookupTransform ( 161 | target_frame, 162 | ros::Time(0), mesh_in.header.frame_id, 163 | ros::Time(0), 164 | fixed_frame, 165 | transform 166 | ); 167 | } 168 | catch (tf::LookupException &e){ 169 | ROS_ERROR ("%s", e.what ()); 170 | return false; 171 | } 172 | catch (tf::ExtrapolationException &e){ 173 | ROS_ERROR ("%s", e.what ()); 174 | return false; 175 | } 176 | 177 | tf::Quaternion quaternion = transform.getRotation (); 178 | Eigen::Quaterniond rotation ( 179 | quaternion.w (), 180 | quaternion.x (), 181 | quaternion.y (), 182 | quaternion.z () 183 | ); 184 | 185 | tf::Vector3 origin = transform.getOrigin (); 186 | Eigen::Vector3d eigen_origin; 187 | vectorTfToEigen(origin, eigen_origin); 188 | Eigen::Translation3d translation ( eigen_origin ); 189 | Eigen::Affine3d transformation = translation * rotation; 190 | 191 | if (&mesh_in != &mesh_out){ 192 | mesh_out.header = mesh_in.header; 193 | mesh_out.header.stamp = ros::Time::now(); 194 | mesh_out.mesh_geometry.faces = mesh_in.mesh_geometry.faces; 195 | } 196 | 197 | mesh_out.mesh_geometry.vertices.resize(mesh_in.mesh_geometry.vertices.size()); 198 | mesh_out.mesh_geometry.vertex_normals.resize(mesh_in.mesh_geometry.vertex_normals.size()); 199 | 200 | // transform vertices 201 | for(size_t i = 0; i < mesh_in.mesh_geometry.vertices.size(); i++) 202 | { 203 | Eigen::Vector3d in_vec, out_vec; 204 | pointMsgToEigen(mesh_in.mesh_geometry.vertices[i], in_vec); 205 | out_vec = transformation * in_vec; 206 | pointEigenToMsg(out_vec, mesh_out.mesh_geometry.vertices[i]); 207 | } 208 | 209 | // rotate normals 210 | for(size_t i = 0; i < mesh_in.mesh_geometry.vertex_normals.size(); i++) 211 | { 212 | Eigen::Vector3d in_vec, out_vec; 213 | pointMsgToEigen(mesh_in.mesh_geometry.vertex_normals[i], in_vec); 214 | out_vec = transformation.rotation() * in_vec; 215 | pointEigenToMsg(out_vec, mesh_out.mesh_geometry.vertex_normals[i]); 216 | } 217 | 218 | mesh_out.header.frame_id = target_frame; 219 | mesh_out.header.stamp = ros::Time::now(); 220 | 221 | return true; 222 | } 223 | 224 | 225 | bool transformTriangleMesh( 226 | const std::string& target_frame, 227 | const ros::Time& target_time, 228 | const mesh_msgs::TriangleMeshStamped& mesh_in, 229 | const std::string& fixed_frame, 230 | mesh_msgs::TriangleMeshStamped& mesh_out, 231 | const tf::TransformListener& tf_listener 232 | ){ 233 | 234 | tf::StampedTransform transform; 235 | try{ 236 | tf_listener.lookupTransform ( 237 | target_frame, 238 | target_time, mesh_in.header.frame_id, 239 | mesh_in.header.stamp, 240 | fixed_frame, 241 | transform 242 | ); 243 | } 244 | catch (tf::LookupException &e){ 245 | ROS_ERROR ("%s", e.what ()); 246 | return false; 247 | } 248 | catch (tf::ExtrapolationException &e){ 249 | ROS_ERROR ("%s", e.what ()); 250 | return false; 251 | } 252 | 253 | tf::Quaternion quaternion = transform.getRotation (); 254 | Eigen::Quaterniond rotation ( 255 | quaternion.w (), 256 | quaternion.x (), 257 | quaternion.y (), 258 | quaternion.z () 259 | ); 260 | 261 | tf::Vector3 origin = transform.getOrigin (); 262 | Eigen::Vector3d eigen_origin; 263 | vectorTfToEigen(origin, eigen_origin); 264 | Eigen::Translation3d translation ( eigen_origin ); 265 | Eigen::Affine3d transformation = translation * rotation; 266 | 267 | if (&mesh_in != &mesh_out){ 268 | mesh_out.header = mesh_in.header; 269 | mesh_out.mesh.triangles = mesh_in.mesh.triangles; 270 | mesh_out.mesh.vertex_colors = mesh_in.mesh.vertex_colors; 271 | mesh_out.mesh.triangle_colors = mesh_in.mesh.triangle_colors; 272 | mesh_out.mesh.vertex_texture_coords = mesh_in.mesh.vertex_texture_coords; 273 | mesh_out.mesh.face_materials = mesh_in.mesh.face_materials; 274 | mesh_out.mesh.textures = mesh_in.mesh.textures; 275 | mesh_out.mesh.clusters = mesh_in.mesh.clusters; 276 | } 277 | 278 | mesh_out.mesh.vertices.resize(mesh_in.mesh.vertices.size()); 279 | mesh_out.mesh.vertex_normals.resize(mesh_in.mesh.vertex_normals.size()); 280 | 281 | // transform vertices 282 | for(size_t i = 0; i < mesh_in.mesh.vertices.size(); i++) 283 | { 284 | Eigen::Vector3d in_vec, out_vec; 285 | pointMsgToEigen(mesh_in.mesh.vertices[i], in_vec); 286 | out_vec = transformation * in_vec; 287 | pointEigenToMsg(out_vec, mesh_out.mesh.vertices[i]); 288 | } 289 | 290 | // rotate normals 291 | for(size_t i = 0; i < mesh_in.mesh.vertex_normals.size(); i++) 292 | { 293 | Eigen::Vector3d in_vec, out_vec; 294 | pointMsgToEigen(mesh_in.mesh.vertex_normals[i], in_vec); 295 | out_vec = transformation.rotation() * in_vec; 296 | pointEigenToMsg(out_vec, mesh_out.mesh.vertex_normals[i]); 297 | } 298 | 299 | mesh_out.header.frame_id = target_frame; 300 | mesh_out.header.stamp = target_time; 301 | 302 | return true; 303 | } 304 | 305 | } /* namespace mesh_msgs_transforms */ 306 | -------------------------------------------------------------------------------- /rviz_map_plugin/src/MeshDisplay.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 | * MeshDisplay.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 | 58 | 59 | namespace rviz_map_plugin 60 | { 61 | 62 | MeshDisplay::MeshDisplay(): rviz::Display() 63 | { 64 | 65 | // Display type selection dropdown 66 | m_displayType = new rviz::EnumProperty( 67 | "Display Type", 68 | "Fixed Color", 69 | "Select Display Type for Mesh", 70 | this, 71 | SLOT(updateMesh()), 72 | this 73 | ); 74 | m_displayType->addOption("Fixed Color", 0); 75 | m_displayType->addOption("Vertex Color", 1); 76 | m_displayType->addOption("Textures", 2); 77 | m_displayType->addOption("Vertex Costs", 3); 78 | m_displayType->addOption("Hide Faces", 4); 79 | 80 | m_showTexturedFacesOnly = new rviz::BoolProperty( 81 | "Show textured faces only", 82 | false, 83 | "Show textured faces only", 84 | m_displayType, 85 | SLOT(updateMesh()), 86 | this 87 | ); 88 | 89 | // face color properties 90 | m_facesColor = new rviz::ColorProperty( 91 | "Faces Color", 92 | QColor(0, 255, 0), 93 | "The color of the faces.", 94 | m_displayType, 95 | SLOT(updateMesh()), 96 | this 97 | ); 98 | 99 | // face alpha properties 100 | m_facesAlpha = new rviz::FloatProperty( 101 | "Faces Alpha", 102 | 1.0, 103 | "The alpha-value of the faces", 104 | m_displayType, 105 | SLOT(updateMesh()), 106 | this 107 | ); 108 | m_facesAlpha->setMin(0); 109 | m_facesAlpha->setMax(1); 110 | 111 | 112 | m_showWireframe = new rviz::BoolProperty( 113 | "Show Wireframe", 114 | true, 115 | "Show Wireframe", 116 | this, 117 | SLOT(updateWireframe()), 118 | this 119 | ); 120 | 121 | // wireframe color property 122 | m_wireframeColor = new rviz::ColorProperty( 123 | "Wireframe Color", 124 | QColor(0, 0, 0), 125 | "The color of the wireframe.", 126 | m_showWireframe, 127 | SLOT(updateWireframe()), 128 | this 129 | ); 130 | // wireframe alpha property 131 | m_wireframeAlpha = new rviz::FloatProperty( 132 | "Wireframe Alpha", 133 | 1.0, 134 | "The alpha-value of the wireframe", 135 | m_showWireframe, 136 | SLOT(updateWireframe()), 137 | this 138 | ); 139 | m_wireframeAlpha->setMin(0); 140 | m_wireframeAlpha->setMax(1); 141 | 142 | 143 | m_showNormals = new rviz::BoolProperty( 144 | "Show Normals", 145 | true, 146 | "Show Normals", 147 | this, 148 | SLOT(updateNormals()), 149 | this 150 | ); 151 | 152 | m_normalsColor = new rviz::ColorProperty( 153 | "Normals Color", 154 | QColor(255, 0, 255), 155 | "The color of the normals.", 156 | m_showNormals, 157 | SLOT(updateNormals()), 158 | this 159 | ); 160 | m_normalsAlpha = new rviz::FloatProperty( 161 | "Normals Alpha", 162 | 1.0, 163 | "The alpha-value of the normals", 164 | m_showNormals, 165 | SLOT(updateNormals()), 166 | this 167 | ); 168 | m_normalsAlpha->setMin(0); 169 | m_normalsAlpha->setMax(1); 170 | m_scalingFactor = new rviz::FloatProperty( 171 | "Normals Scaling Factor", 172 | 0.1, 173 | "Scaling factor of the normals", 174 | m_showNormals, 175 | SLOT(updateNormals()), 176 | this 177 | ); 178 | 179 | setStatus(rviz::StatusProperty::Error, "Display", "Can't be used without Map3D plugin or no data is available"); 180 | } 181 | 182 | MeshDisplay::~MeshDisplay() 183 | { 184 | } 185 | 186 | 187 | // ===================================================================================================================== 188 | // Callbacks 189 | 190 | void MeshDisplay::onInitialize() 191 | { 192 | updateMesh(); 193 | updateWireframe(); 194 | updateNormals(); 195 | } 196 | 197 | void MeshDisplay::onEnable() 198 | { 199 | // Create the visual 200 | updateMesh(); 201 | updateWireframe(); 202 | updateNormals(); 203 | } 204 | 205 | void MeshDisplay::onDisable() 206 | { 207 | if (m_visual) 208 | { 209 | m_visual->updateMaterial( 210 | false, 211 | Ogre::ColourValue(), 212 | 0.0f, 213 | false, 214 | false, 215 | false, 216 | false 217 | ); 218 | m_visual->updateWireframe(false, Ogre::ColourValue(), 0.0f); 219 | m_visual->updateNormals( 220 | false, 221 | Ogre::ColourValue(), 222 | 0.0f, 223 | 1.0f 224 | ); 225 | } 226 | } 227 | 228 | // ===================================================================================================================== 229 | // Callbacks triggered from UI events (mostly) 230 | 231 | void MeshDisplay::updateMesh() 232 | { 233 | ROS_INFO("Mesh Display: Update"); 234 | 235 | bool showFaces = false; 236 | bool showTextures = false; 237 | bool showVertexColors = false; 238 | bool showVertexCosts = false; 239 | 240 | m_showTexturedFacesOnly->hide(); 241 | m_facesColor->hide(); 242 | m_facesAlpha->hide(); 243 | 244 | switch (m_displayType->getOptionInt()) 245 | { 246 | default: 247 | case 0: // Faces with fixed color 248 | showFaces = true; 249 | m_facesColor->show(); 250 | m_facesAlpha->show(); 251 | break; 252 | case 1: // Faces with vertex color 253 | showFaces = true; 254 | showVertexColors = true; 255 | break; 256 | case 2: // Faces with textures 257 | showFaces = true; 258 | showTextures = true; 259 | m_showTexturedFacesOnly->show(); 260 | break; 261 | case 3: // Faces with vertex costs 262 | showFaces = true; 263 | showVertexCosts = true; 264 | break; 265 | case 4: // No Faces 266 | break; 267 | } 268 | 269 | if (!has_data) 270 | { 271 | ROS_ERROR("Mesh display: no data available, can't draw mesh!"); 272 | return; 273 | } 274 | 275 | if (isEnabled()) 276 | { 277 | m_visual->updateMaterial( 278 | showFaces, 279 | m_facesColor->getOgreColor(), 280 | m_facesAlpha->getFloat(), 281 | showVertexColors, 282 | showVertexCosts, 283 | showTextures, 284 | m_showTexturedFacesOnly->getBool() 285 | ); 286 | } 287 | 288 | } 289 | 290 | void MeshDisplay::updateWireframe() 291 | { 292 | bool showWireframe = m_showWireframe->getBool(); 293 | 294 | if (m_visual) 295 | { 296 | m_visual->updateWireframe(showWireframe, m_wireframeColor->getOgreColor(), m_wireframeAlpha->getFloat()); 297 | } 298 | 299 | } 300 | 301 | void MeshDisplay::updateNormals() 302 | { 303 | bool showNormals = m_showNormals->getBool(); 304 | 305 | if (m_visual) 306 | { 307 | m_visual->updateNormals( 308 | showNormals, 309 | m_normalsColor->getOgreColor(), 310 | m_normalsAlpha->getFloat(), 311 | m_scalingFactor->getFloat() 312 | ); 313 | } 314 | 315 | } 316 | 317 | 318 | // ===================================================================================================================== 319 | // Data loading 320 | 321 | void MeshDisplay::setGeometry(shared_ptr geometry) 322 | { 323 | m_geometry = geometry; 324 | 325 | // Create the visual 326 | int randomId = (int)((double)rand() / RAND_MAX * 9998); 327 | m_visual.reset(new TexturedMeshVisual(context_, 0, 0, randomId)); 328 | 329 | m_visual->setGeometry(*geometry); 330 | has_data = true; 331 | if (isEnabled()) 332 | { 333 | updateMesh(); 334 | updateNormals(); 335 | updateWireframe(); 336 | } 337 | setStatus(rviz::StatusProperty::Ok, "Display", ""); 338 | } 339 | 340 | void MeshDisplay::setVertexColors(vector& vertexColors) 341 | { 342 | if (has_data) 343 | { 344 | m_visual->setVertexColors(vertexColors); 345 | } 346 | updateMesh(); 347 | } 348 | 349 | void MeshDisplay::setVertexNormals(vector& vertexNormals) 350 | { 351 | if (has_data) 352 | { 353 | m_visual->setNormals(vertexNormals); 354 | } 355 | if (isEnabled()) 356 | { 357 | updateNormals(); 358 | } 359 | } 360 | 361 | void MeshDisplay::setMaterials(vector& materials, vector& texCoords) 362 | { 363 | if (has_data) 364 | { 365 | m_visual->setMaterials(materials, texCoords); 366 | } 367 | updateMesh(); 368 | } 369 | 370 | void MeshDisplay::addTexture(Texture& texture, uint32_t textureIndex) 371 | { 372 | if (has_data) 373 | { 374 | m_visual->addTexture(texture, textureIndex); 375 | } 376 | } 377 | 378 | } // End namespace rviz_map_plugin 379 | 380 | #include 381 | PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MeshDisplay, rviz::Display) 382 | -------------------------------------------------------------------------------- /rviz_map_plugin/include/TexturedMeshVisual.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 | * TexturedMeshVisual.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 TEXTURED_MESH_VISUAL_HPP 54 | #define TEXTURED_MESH_VISUAL_HPP 55 | 56 | #include 57 | 58 | #include 59 | 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include 71 | 72 | 73 | namespace Ogre 74 | { 75 | 76 | // Forward declaration 77 | class Vector3; 78 | class Quaternion; 79 | class SceneNode; 80 | class Entity; 81 | 82 | } // End namespace Ogre 83 | 84 | namespace rviz_map_plugin 85 | { 86 | 87 | using std::vector; 88 | 89 | /** 90 | * @brief Class to display mesh data in the main panel of rviz. 91 | */ 92 | class TexturedMeshVisual 93 | { 94 | public: 95 | 96 | /** 97 | * @brief Constructor. 98 | * 99 | * @param context The context that contains the display information. 100 | * @param displayID The display id 101 | * @param meshID The mesh id 102 | * @param randomID random number that will be used as part of the meshes UID 103 | */ 104 | TexturedMeshVisual( 105 | rviz::DisplayContext* context, 106 | size_t displayID, 107 | size_t meshID, 108 | size_t randomID 109 | ); 110 | 111 | /** 112 | * @brief Destructor. 113 | */ 114 | virtual ~TexturedMeshVisual(); 115 | 116 | /** 117 | * @brief Clears whole stored data. 118 | */ 119 | void reset(); 120 | 121 | /** 122 | * @brief Extracts data from the ros-messages and creates meshes. 123 | * 124 | * @param geometry Geometry containing the mesh 125 | */ 126 | bool setGeometry(const Geometry& geometry); 127 | 128 | /** 129 | * @brief Passes the normal data to the mesh visual 130 | * 131 | * @param normals Vector containing the normal data 132 | */ 133 | bool setNormals(const vector& normals); 134 | 135 | /** 136 | * @brief Extracts data from the ros-messages and creates a colored mesh. 137 | * 138 | * @param vertexColors Vector containing the vertex color information 139 | */ 140 | bool setVertexColors(const vector& vertexColors); 141 | 142 | /** 143 | * @brief Extracts data from the ros-messages and creates a colored mesh with colors calculated from vertex costs. 144 | * 145 | * @param vertexCosts Vector containing the vertex cost information 146 | */ 147 | bool setVertexCosts(const vector& vertexCosts); 148 | 149 | /** 150 | * @brief Extracts data from the ros-messages and creates a textured mesh. 151 | * 152 | * @param materials Vector containing all materials 153 | * @param texCoords Vector containing all texture coordinates 154 | */ 155 | bool setMaterials(const vector& materials, const vector& texCoords); 156 | 157 | /** 158 | * @brief Extracts data from the ros-messages and adds textures to the textured mesh. 159 | * 160 | * @param texture Texture containing the texture information and data 161 | * @param textureIndex Index of the texture 162 | */ 163 | bool addTexture(Texture& texture, uint32_t textureIndex); 164 | 165 | /** 166 | * @brief Updates the visible parts of the mesh depending on input from the rviz display. 167 | * 168 | * @param showFaces When TRUE faces are visible 169 | * @param facesColor The color of the faces 170 | * @param facesAlpha The transparency of the faces 171 | * @param useVertexColors When TRUE vertex colors are used 172 | * @param showVertexCosts When TRUE vertex costs are visible 173 | * @param showTextures When TRUE textures are visible 174 | * @param showTexturedFacesOnly When TRUE only textured faces are visible 175 | */ 176 | void updateMaterial( 177 | bool showFaces, 178 | Ogre::ColourValue facesColor, 179 | float facesAlpha, 180 | bool useVertexColors, 181 | bool showVertexCosts, 182 | bool showTextures, 183 | bool showTexturedFacesOnly 184 | ); 185 | 186 | /** 187 | * @brief Updates the normals dynamically. 188 | * 189 | * @param showNormals When TRUE normals are visible 190 | * @param normalsColor The color of the normals 191 | * @param normalsAlpha The transparency of the normals 192 | * @param scalingFactor The factor the normals have to be scaled with 193 | */ 194 | void updateNormals( 195 | bool showNormals, 196 | Ogre::ColourValue normalsColor, 197 | float normalsAlpha, 198 | float scalingFactor 199 | ); 200 | 201 | /** 202 | * @brief Updates the wireframe dynamically. 203 | * 204 | * @param showWireframe When TRUE wireframe is visible 205 | * @param wireframeColor The color of the wireframe 206 | * @param wireframeAlpha The transparency of the wireframe 207 | */ 208 | void updateWireframe( 209 | bool showWireframe, 210 | Ogre::ColourValue wireframeColor, 211 | float wireframeAlpha 212 | ); 213 | 214 | 215 | private: 216 | 217 | void showWireframe( 218 | Ogre::Pass* pass, 219 | Ogre::ColourValue wireframeColor, 220 | float wireframeAlpha 221 | ); 222 | 223 | void showFaces( 224 | Ogre::Pass* pass, 225 | Ogre::ColourValue facesColor, 226 | float facesAlpha, 227 | bool useVertexColors 228 | ); 229 | 230 | void showNormals( 231 | Ogre::Pass* pass, 232 | Ogre::ColourValue normalsColor, 233 | float normalsAlpha 234 | ); 235 | 236 | void showTextures(Ogre::Pass* pass); 237 | 238 | void enteringGeneralTriangleMesh(const Geometry& mesh); 239 | void enteringColoredTriangleMesh( 240 | const Geometry& mesh, 241 | const vector& vertexColors 242 | ); 243 | void enteringTriangleMeshWithVertexCosts( 244 | const Geometry& mesh, 245 | const vector& vertexCosts 246 | ); 247 | void enteringTexturedTriangleMesh( 248 | const Geometry& mesh, 249 | const vector& meshMaterials, 250 | const vector& texCoords 251 | ); 252 | void enteringNormals(const Geometry& mesh, const vector& normals); 253 | 254 | Ogre::PixelFormat getOgrePixelFormatFromRosString(std::string encoding); 255 | 256 | void loadImageIntoTextureMaterial(size_t textureIndex); 257 | 258 | /** 259 | * 260 | * @brief Calculates a color for a given cost value using a spectrum from red to green. 261 | * 262 | * @param cost The cost value (should be within the range 0 - 1) 263 | * 264 | * @return Calculated color 265 | */ 266 | Ogre::ColourValue calculateColorFromCost(float cost); 267 | 268 | bool m_vertex_normals_enabled; 269 | bool m_vertex_colors_enabled; 270 | bool m_vertex_costs_enabled; 271 | bool m_materials_enabled; 272 | bool m_texture_coords_enabled; 273 | bool m_textures_enabled; 274 | 275 | /// Ogre Scenenode 276 | Ogre::SceneNode* m_sceneNode; 277 | 278 | /// The context that contains the display information. 279 | rviz::DisplayContext* m_displayContext; 280 | 281 | /// First ID of the created mesh 282 | size_t m_prefix; 283 | 284 | /// Second ID of the created mesh 285 | size_t m_postfix; 286 | 287 | /// Random ID of the created mesh 288 | size_t m_random; 289 | 290 | /// The mesh-object to display 291 | Ogre::ManualObject* m_mesh; 292 | 293 | /// The manual object to display normals 294 | Ogre::ManualObject* m_normalMesh; 295 | 296 | /// The manual object to display the mesh with vertex costs 297 | Ogre::ManualObject* m_vertexCostsMesh; 298 | 299 | /// The manual object to display the textured mesh 300 | Ogre::ManualObject* m_texturedMesh; 301 | 302 | /// The manual object to display the not textured parts of the textured mesh 303 | Ogre::ManualObject* m_noTexCluMesh; 304 | 305 | /// The textures for the mesh 306 | std::vector m_images; 307 | 308 | /// The material for the textured mesh 309 | Ogre::MaterialPtr m_texturedMeshMaterial; 310 | 311 | /// The material for the general mesh 312 | Ogre::MaterialPtr m_meshGeneralMaterial; 313 | 314 | /// The material for the textured triangle mesh 315 | Ogre::MaterialPtr m_meshTexturedTrianglesMaterial; 316 | 317 | /// The material of the normals 318 | Ogre::MaterialPtr m_normalMaterial; 319 | 320 | /// The materials of the not textured clusters 321 | Ogre::MaterialPtr m_noTexCluMaterial; 322 | 323 | /// The material of the mesh with vertex costs 324 | Ogre::MaterialPtr m_vertexCostMaterial; 325 | 326 | /// The materials of the textures 327 | std::vector m_textureMaterials; 328 | 329 | /// Factor the normal-size is multiplied with. 330 | float m_normalsScalingFactor; 331 | 332 | /// Triangle Mesh contained in the given message 333 | Geometry m_geometry; 334 | 335 | }; 336 | } // End namespace rviz_map_plugin 337 | 338 | #endif 339 | --------------------------------------------------------------------------------