diff --git a/hdf5_map_io/CMakeLists.txt b/hdf5_map_io/CMakeLists.txt index 746fadf..724a47c 100644 --- a/hdf5_map_io/CMakeLists.txt +++ b/hdf5_map_io/CMakeLists.txt @@ -1,48 +1,76 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.8) project(hdf5_map_io) -set(CMAKE_CXX_STANDARD 14) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -find_package(catkin REQUIRED) +# DEFAULT RELEASE +if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt) + if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) + endif() +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) find_package(LVR2 REQUIRED) find_package(MPI) find_package(PkgConfig REQUIRED) -catkin_package( - INCLUDE_DIRS include - LIBRARIES hdf5_map_io - DEPENDS LVR2 MPI -) # HighFive set(HIGHFIVE_EXAMPLES FALSE) set(HIGHFIVE_UNIT_TESTS FALSE) +find_library(LVR2_LIBRARY NAMES lvr2) + + include_directories( include ${LVR2_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/hdf5_map_io.cpp ) -find_library(LVR2_LIBRARY NAMES lvr2) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME} PUBLIC ${LVR2_LIBRARY} ${MPI_CXX_LIBRARIES} + rclcpp::rclcpp ) +target_compile_definitions(${PROJECT_NAME} PRIVATE "HDF5_MAP_IO_BUILDING_DLL") + install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +ament_export_dependencies( + rclcpp ) +ament_package() \ No newline at end of file diff --git a/hdf5_map_io/package.xml b/hdf5_map_io/package.xml index 142d203..525b994 100644 --- a/hdf5_map_io/package.xml +++ b/hdf5_map_io/package.xml @@ -1,5 +1,6 @@ - + + hdf5_map_io 1.1.0 The hdf5_map_io package @@ -8,8 +9,15 @@ http://wiki.ros.org/ros_mesh_tools/hdf5_map_io Sebastian Pütz - catkin + ament_cmake + rclcpp boost lvr2 + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/label_manager/CMakeLists.txt b/label_manager/CMakeLists.txt index bf0409c..a78a4ed 100644 --- a/label_manager/CMakeLists.txt +++ b/label_manager/CMakeLists.txt @@ -1,52 +1,84 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(label_manager) -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs - genmsg - mesh_msgs - message_generation - roscpp - sensor_msgs - std_msgs - tf -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -add_service_files(FILES - DeleteLabel.srv - GetLabelGroups.srv - GetLabeledClusterGroup.srv -) +find_package(ament_cmake REQUIRED) -generate_messages(DEPENDENCIES - mesh_msgs - std_msgs + +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(actionlib_msgs REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(Boost COMPONENTS + system + filesystem ) -catkin_package( - CATKIN_DEPENDS - actionlib actionlib_msgs genmsg mesh_msgs message_generation message_runtime roscpp sensor_msgs std_msgs tf + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/DeleteLabel.srv" + "srv/GetLabelGroups.srv" + "srv/GetLabeledClusterGroup.srv" + DEPENDENCIES + mesh_msgs + std_msgs + ADD_LINTER_TESTS ) + include_directories( - include ${catkin_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + include ) - -add_executable(${PROJECT_NAME} +add_executable(${PROJECT_NAME}_node src/manager.cpp src/manager_node.cpp) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + +add_dependencies(${PROJECT_NAME}_node + ${PROJECT_NAME} +) + +target_link_libraries(${PROJECT_NAME}_node + Boost::system + Boost::filesystem +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + sensor_msgs + std_msgs ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) +rosidl_get_typesupport_target(cpp_typesupport_target + ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${PROJECT_NAME}_node + "${cpp_typesupport_target}") + + +ament_export_dependencies(rosidl_default_runtime) +ament_package() + diff --git a/label_manager/include/label_manager/manager.h b/label_manager/include/label_manager/manager.h index 263f711..754cfc2 100644 --- a/label_manager/include/label_manager/manager.h +++ b/label_manager/include/label_manager/manager.h @@ -2,46 +2,63 @@ #define LABEL_MANAGER_H_ #include +#include +#include +#include +#include + +#include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" +#include "mesh_msgs/srv/get_labeled_clusters.hpp" +#include "label_manager/srv/get_label_groups.hpp" +#include "label_manager/srv/get_labeled_cluster_group.hpp" +#include "label_manager/srv/delete_label.hpp" + +#include "rclcpp/rclcpp.hpp" -#include -#include -#include -#include -#include -#include namespace label_manager { -class LabelManager +class LabelManager : public rclcpp::Node { public: - LabelManager(ros::NodeHandle& nodeHandle); + LabelManager(std::string handle_str = ""); private: - ros::NodeHandle nh; - ros::Subscriber clusterLabelSub; - ros::Publisher newClusterLabelPub; - ros::ServiceServer srv_get_labeled_clusters; - ros::ServiceServer srv_get_label_groups; - ros::ServiceServer srv_get_labeled_cluster_group; - ros::ServiceServer srv_delete_label; + // Subscriber + rclcpp::Subscription::SharedPtr + clusterLabelSub; + + // Publisher + rclcpp::Publisher::SharedPtr + newClusterLabelPub; + // Service (Servers) + rclcpp::Service::SharedPtr + srv_get_labeled_clusters; + rclcpp::Service::SharedPtr + srv_get_label_groups; + rclcpp::Service::SharedPtr + srv_get_labeled_cluster_group; + rclcpp::Service::SharedPtr + srv_delete_label; + std::string folderPath; - void clusterLabelCallback(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg); + void clusterLabelCallback(const mesh_msgs::msg::MeshFaceClusterStamped& msg); + bool service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request& req, - mesh_msgs::GetLabeledClusters::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getLabelGroups( - label_manager::GetLabelGroups::Request& req, - label_manager::GetLabelGroups::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getLabeledClusterGroup( - label_manager::GetLabeledClusterGroup::Request& req, - label_manager::GetLabeledClusterGroup::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool service_deleteLabel( - label_manager::DeleteLabel::Request& req, - label_manager::DeleteLabel::Response& res); + const std::shared_ptr req, + std::shared_ptr res); bool writeIndicesToFile(const std::string& fileName, const std::vector& indices, const bool append); std::vector readIndicesFromFile(const std::string& fileName); diff --git a/label_manager/package.xml b/label_manager/package.xml index 72d5ca2..6e09693 100644 --- a/label_manager/package.xml +++ b/label_manager/package.xml @@ -1,9 +1,9 @@ - + + + label_manager - - Serving and persisting label information - 1.1.0 + Serving and persisting label information Sebastian Pütz http://wiki.ros.org/ros_mesh_tools/label_manager @@ -13,27 +13,25 @@ BSD-3 - actionlib_msgs - actionlib - genmsg - mesh_msgs - message_generation - roscpp - sensor_msgs - std_msgs - tf - - actionlib_msgs - actionlib - genmsg - mesh_msgs - message_generation - message_runtime - roscpp - sensor_msgs - std_msgs - tf - - catkin + ament_cmake + rosidl_default_generators + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + sensor_msgs + std_msgs + + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + diff --git a/label_manager/src/manager.cpp b/label_manager/src/manager.cpp index e2310b8..99fdad9 100644 --- a/label_manager/src/manager.cpp +++ b/label_manager/src/manager.cpp @@ -4,76 +4,72 @@ #include #include #include -#include "mesh_msgs/MeshFaceCluster.h" +#include "mesh_msgs/msg/mesh_face_cluster.h" using namespace boost::filesystem; +using std::placeholders::_1; +using std::placeholders::_2; + namespace label_manager { - LabelManager::LabelManager(ros::NodeHandle& nodeHandle) : - nh(nodeHandle) { - - if (!nh.getParam("folder_path", folderPath)) - { - folderPath = "/tmp/label_manager/"; - } + LabelManager::LabelManager(std::string handle_str) + :rclcpp::Node(handle_str) + { + // TODO: check if this is correct + this->declare_parameter("folder_path", "/tmp/label_manager/"); + folderPath = this->get_parameter("folder_path").as_string(); + path p(folderPath); - if (!is_directory(p) && !exists(p)) + if(!is_directory(p) && !exists(p)) { create_directory(p); } - clusterLabelSub = nh.subscribe("cluster_label", 10, &LabelManager::clusterLabelCallback, this); - newClusterLabelPub = nh.advertise("new_cluster_label", 1); - srv_get_labeled_clusters = nh.advertiseService( - "get_labeled_clusters", - &LabelManager::service_getLabeledClusters, - this - ); - srv_get_label_groups = nh.advertiseService( - "get_label_groups", - &LabelManager::service_getLabelGroups, - this - ); - srv_get_labeled_cluster_group = nh.advertiseService( - "get_labeled_cluster_group", - &LabelManager::service_getLabeledClusterGroup, - this - ); - srv_delete_label = nh.advertiseService( - "delete_label", - &LabelManager::service_deleteLabel, - this - ); - - ROS_INFO("Started LabelManager"); - - ros::spin(); + clusterLabelSub = this->create_subscription( + "cluster_label", 10, std::bind(&LabelManager::clusterLabelCallback, this, _1)); + + newClusterLabelPub = this->create_publisher( + "new_cluster_label", 10); + + srv_get_labeled_clusters = this->create_service( + "get_labeled_clusters", std::bind(&LabelManager::service_getLabeledClusters, this, _1, _2)); + + srv_get_label_groups = this->create_service( + "get_label_groups", std::bind(&LabelManager::service_getLabelGroups, this, _1, _2)); + + srv_get_labeled_cluster_group = this->create_service( + "get_labeled_cluster_group", std::bind(&LabelManager::service_getLabeledClusterGroup, this, _1, _2)); + + srv_delete_label = this->create_service( + "delete_label", std::bind(&LabelManager::service_deleteLabel, this, _1, _2)); + + RCLCPP_INFO(this->get_logger(), "Started LabelManager"); } - void LabelManager::clusterLabelCallback(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg) + void LabelManager::clusterLabelCallback(const mesh_msgs::msg::MeshFaceClusterStamped& msg) { - ROS_INFO_STREAM("Got msg for mesh: " << msg->uuid << " with label: " << msg->cluster.label); + RCLCPP_INFO_STREAM(this->get_logger(), "Got msg for mesh: " << msg.uuid << " with label: " << msg.cluster.label); std::vector indices; - std::string fileName = getFileName(msg->uuid, msg->cluster.label); + std::string fileName = getFileName(msg.uuid, msg.cluster.label); // if appending (not override), first figure what new indices we have to add - if (!msg->override) + if (!msg.override) { std::vector readIndices = readIndicesFromFile(fileName); // if read indices is empty no file was found or could not be read if (readIndices.empty()) { - indices = msg->cluster.face_indices; + indices = msg.cluster.face_indices; } else { - for (size_t i = 0; i < msg->cluster.face_indices.size(); i++) + for (size_t i = 0; i < msg.cluster.face_indices.size(); i++) { - uint idx = msg->cluster.face_indices[i]; + uint idx = msg.cluster.face_indices[i]; // if msg index is not already in file, add it to indices vector if (std::find(readIndices.begin(), readIndices.end(), idx) == readIndices.end()) @@ -85,35 +81,35 @@ namespace label_manager } else { - indices = msg->cluster.face_indices; + indices = msg.cluster.face_indices; } // publish every new labeled cluster - newClusterLabelPub.publish(msg->cluster); + newClusterLabelPub->publish(msg.cluster); // make sure mesh folder exists before writing - path p(folderPath + "/" + msg->uuid); + path p(folderPath + "/" + msg.uuid); if (!is_directory(p) || !exists(p)) { create_directory(p); } - writeIndicesToFile(fileName, indices, !msg->override); + writeIndicesToFile(fileName, indices, !msg.override); } bool LabelManager::service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request& req, - mesh_msgs::GetLabeledClusters::Response& res - ) + const std::shared_ptr req, + std::shared_ptr res) { - ROS_DEBUG_STREAM("Service call with uuid: " << req.uuid); - path p (folderPath + "/" + req.uuid); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Service call with uuid: " << req->uuid); + + path p (folderPath + "/" + req->uuid); directory_iterator end_itr; if (!is_directory(p) || !exists(p)) { - ROS_DEBUG_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); + RCLCPP_DEBUG_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); return false; } @@ -127,11 +123,11 @@ namespace label_manager // remove extension from label boost::replace_all(label, itr->path().filename().extension().string(), ""); - mesh_msgs::MeshFaceCluster c; + mesh_msgs::msg::MeshFaceCluster c; c.face_indices = readIndicesFromFile(itr->path().string()); c.label = label; - res.clusters.push_back(c); + res->clusters.push_back(c); } } @@ -139,15 +135,15 @@ namespace label_manager } bool LabelManager::service_getLabelGroups( - label_manager::GetLabelGroups::Request& req, - label_manager::GetLabelGroups::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { - path p (folderPath + "/" + req.uuid); + path p (folderPath + "/" + req->uuid); directory_iterator end_itr; if (!is_directory(p) || !exists(p)) { - ROS_WARN_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); + RCLCPP_WARN_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); return false; } @@ -167,9 +163,9 @@ namespace label_manager label = label.substr(0, label.find_first_of("_", 0)); // only add label group to response if not already added - if (std::find(res.labels.begin(), res.labels.end(), label) == res.labels.end()) + if (std::find(res->labels.begin(), res->labels.end(), label) == res->labels.end()) { - res.labels.push_back(label); + res->labels.push_back(label); } } } @@ -178,35 +174,16 @@ namespace label_manager return true; } - bool LabelManager::service_deleteLabel( - label_manager::DeleteLabel::Request& req, - label_manager::DeleteLabel::Response& res) - { - path p(getFileName(req.uuid, req.label)); - - if (!is_regular_file(p) || !exists(p)) - { - ROS_WARN_STREAM("Could not delete label '" << req.label << "' of mesh '" << req.uuid << "'."); - - return false; - } - - res.cluster.face_indices = readIndicesFromFile(p.filename().string()); - res.cluster.label = req.label; - - return remove(p); - } - bool LabelManager::service_getLabeledClusterGroup( - label_manager::GetLabeledClusterGroup::Request& req, - label_manager::GetLabeledClusterGroup::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { - path p (folderPath + "/" + req.uuid); + path p (folderPath + "/" + req->uuid); directory_iterator end_itr; if (!is_directory(p) || !exists(p)) { - ROS_WARN_STREAM("No labeled clusters for uuid '" << req.uuid << "' found"); + RCLCPP_WARN_STREAM(this->get_logger(), "No labeled clusters for uuid '" << req->uuid << "' found"); return false; } @@ -214,17 +191,17 @@ namespace label_manager for (directory_iterator itr(p); itr != end_itr; ++itr) { // if file is no dir - if (is_regular_file(itr->path()) && itr->path().filename().string().find(req.labelGroup) == 0) + if (is_regular_file(itr->path()) && itr->path().filename().string().find(req->label_group) == 0) { std::string label = itr->path().filename().string(); // remove extension from label boost::replace_all(label, itr->path().filename().extension().string(), ""); - mesh_msgs::MeshFaceCluster c; + mesh_msgs::msg::MeshFaceCluster c; c.face_indices = readIndicesFromFile(itr->path().string()); c.label = label; - res.clusters.push_back(c); + res->clusters.push_back(c); } } @@ -232,6 +209,25 @@ namespace label_manager return true; } + bool LabelManager::service_deleteLabel( + const std::shared_ptr req, + std::shared_ptr res) + { + path p(getFileName(req->uuid, req->label)); + + if (!is_regular_file(p) || !exists(p)) + { + RCLCPP_WARN_STREAM(this->get_logger(), "Could not delete label '" << req->label << "' of mesh '" << req->uuid << "'."); + + return false; + } + + res->cluster.face_indices = readIndicesFromFile(p.filename().string()); + res->cluster.label = req->label; + + return remove(p); + } + bool LabelManager::writeIndicesToFile( const std::string& fileName, const std::vector& indices, @@ -240,7 +236,7 @@ namespace label_manager { if (indices.empty()) { - ROS_WARN_STREAM("Empty indices."); + RCLCPP_WARN_STREAM(this->get_logger(), "Empty indices."); return true; } @@ -248,7 +244,7 @@ namespace label_manager std::ios_base::openmode mode = append ? (std::ios::out|std::ios::app) : std::ios::out; std::ofstream ofs(fileName.c_str(), mode); - ROS_DEBUG_STREAM("Writing indices to file: " << fileName); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Writing indices to file: " << fileName); if (ofs.is_open()) { @@ -270,13 +266,13 @@ namespace label_manager } ofs.close(); - ROS_DEBUG_STREAM("Successfully written indices to file."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Successfully written indices to file."); return true; } else { - ROS_ERROR_STREAM("Could not open file: " << fileName); + RCLCPP_ERROR_STREAM(this->get_logger(), "Could not open file: " << fileName); } return false; @@ -290,7 +286,7 @@ namespace label_manager // if file dos not exists, return empty vector if (!ifs.good()) { - ROS_DEBUG_STREAM("File " << fileName << " does not exists. Nothing to read..."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "File " << fileName << " does not exists. Nothing to read..."); return faceIndices; } diff --git a/label_manager/src/manager_node.cpp b/label_manager/src/manager_node.cpp index d18cd97..ac5fff9 100644 --- a/label_manager/src/manager_node.cpp +++ b/label_manager/src/manager_node.cpp @@ -3,16 +3,14 @@ * */ -#include +#include "rclcpp/rclcpp.hpp" #include "label_manager/manager.h" int main(int argc, char **argv) { - ros::init(argc, argv, "label_manager"); - ros::NodeHandle nodeHandle("~"); - - label_manager::LabelManager lm(nodeHandle); - - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared( + "label_manager")); + rclcpp::shutdown(); return 0; } diff --git a/label_manager/srv/GetLabeledClusterGroup.srv b/label_manager/srv/GetLabeledClusterGroup.srv index 9591e20..67d4ef3 100644 --- a/label_manager/srv/GetLabeledClusterGroup.srv +++ b/label_manager/srv/GetLabeledClusterGroup.srv @@ -1,4 +1,4 @@ string uuid -string labelGroup +string label_group --- mesh_msgs/MeshFaceCluster[] clusters diff --git a/mesh_msgs/CMakeLists.txt b/mesh_msgs/CMakeLists.txt index f43942d..3848de6 100644 --- a/mesh_msgs/CMakeLists.txt +++ b/mesh_msgs/CMakeLists.txt @@ -1,61 +1,53 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(mesh_msgs) -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - message_generation - roscpp - sensor_msgs - std_msgs -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -add_message_files( - FILES - MeshFaceCluster.msg - MeshFaceClusterStamped.msg - MeshMaterial.msg - MeshGeometry.msg - MeshGeometryStamped.msg - MeshMaterials.msg - MeshMaterialsStamped.msg - MeshVertexColors.msg - MeshVertexColorsStamped.msg - MeshVertexCosts.msg - MeshVertexCostsStamped.msg - MeshTexture.msg - MeshTriangleIndices.msg - VectorField.msg - VectorFieldStamped.msg - MeshVertexTexCoords.msg -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) -add_service_files( - DIRECTORY - service - FILES - GetGeometry.srv - GetLabeledClusters.srv - GetMaterials.srv - GetTexture.srv - GetUUIDs.srv - GetVertexColors.srv - GetVertexCosts.srv - GetVertexCostLayers.srv -) -generate_messages( +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MeshFaceCluster.msg" + "msg/MeshFaceClusterStamped.msg" + "msg/MeshMaterial.msg" + "msg/MeshGeometry.msg" + "msg/MeshGeometryStamped.msg" + "msg/MeshMaterials.msg" + "msg/MeshMaterialsStamped.msg" + "msg/MeshVertexColors.msg" + "msg/MeshVertexColorsStamped.msg" + "msg/MeshVertexCosts.msg" + "msg/MeshVertexCostsStamped.msg" + "msg/MeshTexture.msg" + "msg/MeshTriangleIndices.msg" + "msg/VectorField.msg" + "msg/VectorFieldStamped.msg" + "msg/MeshVertexTexCoords.msg" + "srv/GetGeometry.srv" + "srv/GetLabeledClusters.srv" + "srv/GetMaterials.srv" + "srv/GetTexture.srv" + "srv/GetUUIDs.srv" + "srv/GetVertexColors.srv" + "srv/GetVertexCosts.srv" + "srv/GetVertexCostLayers.srv" DEPENDENCIES - geometry_msgs - sensor_msgs - std_msgs -) - -catkin_package( - INCLUDE_DIRS - CATKIN_DEPENDS + std_msgs geometry_msgs - message_runtime - roscpp sensor_msgs - std_msgs + ADD_LINTER_TESTS ) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() \ No newline at end of file diff --git a/mesh_msgs/package.xml b/mesh_msgs/package.xml index 6a824ff..0e8e280 100644 --- a/mesh_msgs/package.xml +++ b/mesh_msgs/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs 1.1.0 Various Message Types for Mesh Data. @@ -13,17 +14,20 @@ http://wiki.ros.org/mesh_tools/mesh_msgs BSD-3 - catkin + ament_cmake + rosidl_default_generators - roscpp - message_generation - geometry_msgs - sensor_msgs - std_msgs - - roscpp - message_runtime - geometry_msgs - sensor_msgs - std_msgs + geometry_msgs + sensor_msgs + std_msgs + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + diff --git a/mesh_msgs/service/GetGeometry.srv b/mesh_msgs/srv/GetGeometry.srv similarity index 100% rename from mesh_msgs/service/GetGeometry.srv rename to mesh_msgs/srv/GetGeometry.srv diff --git a/mesh_msgs/service/GetLabeledClusters.srv b/mesh_msgs/srv/GetLabeledClusters.srv similarity index 100% rename from mesh_msgs/service/GetLabeledClusters.srv rename to mesh_msgs/srv/GetLabeledClusters.srv diff --git a/mesh_msgs/service/GetMaterials.srv b/mesh_msgs/srv/GetMaterials.srv similarity index 100% rename from mesh_msgs/service/GetMaterials.srv rename to mesh_msgs/srv/GetMaterials.srv diff --git a/mesh_msgs/service/GetTexture.srv b/mesh_msgs/srv/GetTexture.srv similarity index 100% rename from mesh_msgs/service/GetTexture.srv rename to mesh_msgs/srv/GetTexture.srv diff --git a/mesh_msgs/service/GetUUIDs.srv b/mesh_msgs/srv/GetUUIDs.srv similarity index 100% rename from mesh_msgs/service/GetUUIDs.srv rename to mesh_msgs/srv/GetUUIDs.srv diff --git a/mesh_msgs/service/GetVertexColors.srv b/mesh_msgs/srv/GetVertexColors.srv similarity index 100% rename from mesh_msgs/service/GetVertexColors.srv rename to mesh_msgs/srv/GetVertexColors.srv diff --git a/mesh_msgs/service/GetVertexCostLayers.srv b/mesh_msgs/srv/GetVertexCostLayers.srv similarity index 100% rename from mesh_msgs/service/GetVertexCostLayers.srv rename to mesh_msgs/srv/GetVertexCostLayers.srv diff --git a/mesh_msgs/service/GetVertexCosts.srv b/mesh_msgs/srv/GetVertexCosts.srv similarity index 100% rename from mesh_msgs/service/GetVertexCosts.srv rename to mesh_msgs/srv/GetVertexCosts.srv diff --git a/mesh_msgs_conversions/CMakeLists.txt b/mesh_msgs_conversions/CMakeLists.txt index 5dfe004..9e4a931 100644 --- a/mesh_msgs_conversions/CMakeLists.txt +++ b/mesh_msgs_conversions/CMakeLists.txt @@ -1,14 +1,21 @@ -cmake_minimum_required(VERSION 3.1) - +cmake_minimum_required(VERSION 3.8) project(mesh_msgs_conversions) -set(PACKAGE_DEPENDENCIES - mesh_msgs - roscpp - sensor_msgs -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) find_package(LVR2 REQUIRED) find_package(OpenCV REQUIRED) find_package(MPI REQUIRED) @@ -16,8 +23,8 @@ find_package(PkgConfig REQUIRED) add_definitions(${LVR2_DEFINITIONS} ${OpenCV_DEFINITIONS}) -### compile with c++14 -set (CMAKE_CXX_STANDARD 14) + +find_library(LVR2_LIBRARY NAMES lvr2) # enable openmp support #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") @@ -29,33 +36,47 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) -catkin_package( - CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} - INCLUDE_DIRS include - DEPENDS LVR2 MPI - LIBRARIES ${PROJECT_NAME} -) - add_library(${PROJECT_NAME} src/conversions.cpp ) -find_library(LVR2_LIBRARY NAMES lvr2) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +target_link_libraries(${PROJECT_NAME} PUBLIC ${LVR2_LIBRARY} ${OpenCV_LIBRARIES} + rclcpp::rclcpp + ${mesh_msgs_TARGETS} + ${sensor_msgs_TARGETS} ) -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_MSGS_CONVERSIONS_BUILDING_DLL") + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +ament_export_dependencies( + rclcpp + mesh_msgs + sensor_msgs ) + +ament_package() \ No newline at end of file diff --git a/mesh_msgs_conversions/README.md b/mesh_msgs_conversions/README.md new file mode 100644 index 0000000..78a0d8b --- /dev/null +++ b/mesh_msgs_conversions/README.md @@ -0,0 +1,15 @@ +# mesh_msgs_conversions + + +Conversion functions between lvr2 and mesh_msgs. + + +## ROS2 Port TODOs + +No global nodes anymore + +I changed +1. ros::Time::now() -> rclcpp::Time() +2. ROS_INFO -> std::cout + +The first point could destroy functionallity. The second one could destroy the logging. TODO: do this correctly \ No newline at end of file diff --git a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h index 7cc7f23..c594a59 100644 --- a/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h +++ b/mesh_msgs_conversions/include/mesh_msgs_conversions/conversions.h @@ -32,8 +32,7 @@ #include -#include -#include +#include "rclcpp/rclcpp.hpp" #include #include @@ -49,25 +48,25 @@ #include #include -#include -#include -#include +#include "std_msgs/msg/string.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/fill_image.hpp" -#include -#include -#include +#include "mesh_msgs/msg/mesh_face_cluster.hpp" +#include "mesh_msgs/msg/mesh_material.hpp" +#include "mesh_msgs/msg/mesh_triangle_indices.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "mesh_msgs/msg/mesh_geometry.hpp" +#include "mesh_msgs/msg/mesh_geometry_stamped.hpp" +#include "mesh_msgs/msg/mesh_materials_stamped.hpp" +#include "mesh_msgs/msg/mesh_vertex_colors.hpp" +#include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp" +#include "mesh_msgs/msg/mesh_vertex_costs_stamped.hpp" +#include "mesh_msgs/msg/mesh_vertex_tex_coords.hpp" +#include "mesh_msgs/msg/mesh_material.hpp" +#include "mesh_msgs/msg/mesh_texture.hpp" -#include +#include "sensor_msgs/point_cloud2_iterator.hpp" namespace mesh_msgs_conversions @@ -91,11 +90,11 @@ typedef boost::shared_ptr MaterialGroupPtr; template -inline const mesh_msgs::MeshGeometry toMeshGeometry( +inline const mesh_msgs::msg::MeshGeometry toMeshGeometry( const lvr2::HalfEdgeMesh>& hem, const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>()) { - mesh_msgs::MeshGeometry mesh_msg; + mesh_msgs::msg::MeshGeometry mesh_msg; mesh_msg.vertices.reserve(hem.numVertices()); mesh_msg.faces.reserve(hem.numFaces()); @@ -109,14 +108,14 @@ inline const mesh_msgs::MeshGeometry toMeshGeometry( { new_indices.insert(vH, k++); const auto& pi = hem.getVertexPosition(vH); - geometry_msgs::Point p; + geometry_msgs::msg::Point p; p.x = pi.x; p.y = pi.y; p.z = pi.z; mesh_msg.vertices.push_back(p); } for(auto fH : hem.faces()) { - mesh_msgs::MeshTriangleIndices indices; + mesh_msgs::msg::MeshTriangleIndices indices; auto vHs = hem.getVerticesOfFace(fH); indices.vertex_indices[0] = new_indices[vHs[0]]; indices.vertex_indices[1] = new_indices[vHs[1]]; @@ -127,7 +126,7 @@ inline const mesh_msgs::MeshGeometry toMeshGeometry( for(auto vH : hem.vertices()) { const auto& n = normals[vH]; - geometry_msgs::Point v; + geometry_msgs::msg::Point v; v.x = n.x; v.y = n.y; v.z = n.z; mesh_msg.vertex_normals.push_back(v); } @@ -136,14 +135,14 @@ inline const mesh_msgs::MeshGeometry toMeshGeometry( } template -inline const mesh_msgs::MeshGeometryStamped toMeshGeometryStamped( +inline const mesh_msgs::msg::MeshGeometryStamped toMeshGeometryStamped( const lvr2::HalfEdgeMesh>& hem, const std::string& frame_id, const std::string& uuid, const lvr2::VertexMap>& normals = lvr2::DenseVertexMap>(), - const ros::Time& stamp = ros::Time::now()) + const rclcpp::Time& stamp = rclcpp::Time()) { - mesh_msgs::MeshGeometryStamped mesh_msg; + mesh_msgs::msg::MeshGeometryStamped mesh_msg; mesh_msg.mesh_geometry = toMeshGeometry(hem, normals); mesh_msg.uuid = uuid; mesh_msg.header.frame_id = frame_id; @@ -151,12 +150,12 @@ inline const mesh_msgs::MeshGeometryStamped toMeshGeometryStamped( return mesh_msg; } -inline const mesh_msgs::MeshVertexCosts toVertexCosts( +inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts( const lvr2::VertexMap& costs, const size_t num_values, const float default_value) { - mesh_msgs::MeshVertexCosts costs_msg; + mesh_msgs::msg::MeshVertexCosts costs_msg; costs_msg.costs.resize(num_values, default_value); for(auto vH : costs){ costs_msg.costs[vH.idx()] = costs[vH]; @@ -164,17 +163,17 @@ inline const mesh_msgs::MeshVertexCosts toVertexCosts( return costs_msg; } -inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( +inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped( const lvr2::VertexMap& costs, const size_t num_values, const float default_value, const std::string& name, const std::string& frame_id, const std::string& uuid, - const ros::Time& stamp = ros::Time::now() + const rclcpp::Time& stamp = rclcpp::Time() ) { - mesh_msgs::MeshVertexCostsStamped mesh_msg; + mesh_msgs::msg::MeshVertexCostsStamped mesh_msg; mesh_msg.mesh_vertex_costs = toVertexCosts(costs, num_values, default_value); mesh_msg.uuid = uuid; mesh_msg.type = name; @@ -183,9 +182,10 @@ inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( return mesh_msg; } -inline const mesh_msgs::MeshVertexCosts toVertexCosts(const lvr2::DenseVertexMap& costs) +inline const mesh_msgs::msg::MeshVertexCosts toVertexCosts( + const lvr2::DenseVertexMap& costs) { - mesh_msgs::MeshVertexCosts costs_msg; + mesh_msgs::msg::MeshVertexCosts costs_msg; costs_msg.costs.reserve(costs.numValues()); for(auto vH : costs){ costs_msg.costs.push_back(costs[vH]); @@ -193,15 +193,15 @@ inline const mesh_msgs::MeshVertexCosts toVertexCosts(const lvr2::DenseVertexMap return costs_msg; } -inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( +inline const mesh_msgs::msg::MeshVertexCostsStamped toVertexCostsStamped( const lvr2::DenseVertexMap& costs, const std::string& name, const std::string& frame_id, const std::string& uuid, - const ros::Time& stamp = ros::Time::now() + const rclcpp::Time& stamp = rclcpp::Time() ) { - mesh_msgs::MeshVertexCostsStamped mesh_msg; + mesh_msgs::msg::MeshVertexCostsStamped mesh_msg; mesh_msg.mesh_vertex_costs = toVertexCosts(costs); mesh_msg.uuid = uuid; mesh_msg.type = name; @@ -213,48 +213,32 @@ inline const mesh_msgs::MeshVertexCostsStamped toVertexCostsStamped( bool fromMeshBufferToMeshGeometryMessage( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry + mesh_msgs::msg::MeshGeometry& mesh_geometry ); /// Convert lvr2::MeshBuffer to various messages for services bool fromMeshBufferToMeshMessages( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry, - mesh_msgs::MeshMaterials& mesh_materials, - mesh_msgs::MeshVertexColors& mesh_vertex_colors, - boost::optional&> texture_cache, + mesh_msgs::msg::MeshGeometry& mesh_geometry, + mesh_msgs::msg::MeshMaterials& mesh_materials, + mesh_msgs::msg::MeshVertexColors& mesh_vertex_colors, + boost::optional&> texture_cache, std::string mesh_uuid ); bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, + const std::shared_ptr mesh_geometry_ptr, lvr2::MeshBufferPtr& buffer_ptr ); bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer -); - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBufferPtr& buffer_ptr ); bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, - lvr2::MeshBufferPtr& buffer_ptr -); - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer -); - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, - lvr2::MeshBuffer& buffer -); + const mesh_msgs::msg::MeshGeometry& mesh_geometry, + lvr2::MeshBuffer& buffer); /* TODO void removeDuplicates(lvr2::MeshBuffer& buffer); @@ -277,7 +261,8 @@ bool readMeshBuffer(lvr2::MeshBufferPtr& buffer, string path); */ bool writeMeshBuffer(lvr2::MeshBufferPtr& mesh, string path); -bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, PointBuffer& buffer); +bool fromPointCloud2ToPointBuffer( + const sensor_msgs::msg::PointCloud2& cloud, PointBuffer& buffer); /** * @brief converts lvr2::Pointbuffer to pointcloud2. @@ -287,7 +272,10 @@ bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, PointBu * @param frame the frame of the converted pointcloud2 * @param cloud the converted pointcloud2 */ -void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string frame, sensor_msgs::PointCloud2Ptr& cloud); +void PointBufferToPointCloud2( + const lvr2::PointBufferPtr& buffer, + std::string frame, + std::shared_ptr& cloud); /** * @brief converts pointcloud2 to a newly created Pointerbuffer. @@ -298,7 +286,9 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr * * @return */ -void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::PointBufferPtr& buffer); +void PointCloud2ToPointBuffer( + const std::shared_ptr cloud, + lvr2::PointBufferPtr& buffer); /** @@ -308,7 +298,7 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po * @return bool success status */ bool fromMeshGeometryMessageToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, const lvr2::MeshBufferPtr& buffer ); diff --git a/mesh_msgs_conversions/package.xml b/mesh_msgs_conversions/package.xml index 2e05138..6f1f31a 100644 --- a/mesh_msgs_conversions/package.xml +++ b/mesh_msgs_conversions/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs_conversions 1.1.0 converts point clouds and attributes into meshes and mesh attributes @@ -10,11 +11,16 @@ Sebastian Pütz Henning Deeken + ament_cmake lvr2 - roscpp + rclcpp sensor_msgs mesh_msgs - catkin + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/mesh_msgs_conversions/src/conversions.cpp b/mesh_msgs_conversions/src/conversions.cpp index 760939e..16a9e65 100644 --- a/mesh_msgs_conversions/src/conversions.cpp +++ b/mesh_msgs_conversions/src/conversions.cpp @@ -36,12 +36,12 @@ namespace mesh_msgs_conversions bool fromMeshBufferToMeshGeometryMessage( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry + mesh_msgs::msg::MeshGeometry& mesh_geometry ){ size_t n_vertices = buffer->numVertices(); size_t n_faces = buffer->numFaces(); - ROS_DEBUG_STREAM("Copy vertices from MeshBuffer to MeshGeometry."); + // RCLCPP_DEBUG_STREAM("Copy vertices from MeshBuffer to MeshGeometry."); // Copy vertices mesh_geometry.vertices.resize(n_vertices); @@ -53,7 +53,7 @@ bool fromMeshBufferToMeshGeometryMessage( mesh_geometry.vertices[i].z = buffer_vertices[i * 3 + 2]; } - ROS_DEBUG_STREAM("Copy faces from MeshBuffer to MeshGeometry."); + // RCLCPP_DEBUG_STREAM("Copy faces from MeshBuffer to MeshGeometry."); // Copy faces auto buffer_faces = buffer->getFaceIndices(); @@ -69,7 +69,7 @@ bool fromMeshBufferToMeshGeometryMessage( auto buffer_vertexnormals = buffer->getVertexNormals(); if(buffer->hasVertexNormals()) { - ROS_DEBUG_STREAM("Copy normals from MeshBuffer to MeshGeometry."); + // RCLCPP_DEBUG_STREAM("Copy normals from MeshBuffer to MeshGeometry."); mesh_geometry.vertex_normals.resize(n_vertices); for (unsigned int i = 0; i < n_vertices; i++) { @@ -78,20 +78,20 @@ bool fromMeshBufferToMeshGeometryMessage( mesh_geometry.vertex_normals[i].z = buffer_vertexnormals[i * 3 + 2]; } }else{ - ROS_DEBUG_STREAM("No vertex normals given!"); + // RCLCPP_DEBUG_STREAM("No vertex normals given!"); } - ROS_DEBUG_STREAM("Successfully copied the MeshBuffer " - "geometry to the MeshGeometry message."); + // RCLCPP_DEBUG_STREAM("Successfully copied the MeshBuffer " + // "geometry to the MeshGeometry message."); return true; } bool fromMeshBufferToMeshMessages( const lvr2::MeshBufferPtr& buffer, - mesh_msgs::MeshGeometry& mesh_geometry, - mesh_msgs::MeshMaterials& mesh_materials, - mesh_msgs::MeshVertexColors& mesh_vertex_colors, - boost::optional&> texture_cache, + mesh_msgs::msg::MeshGeometry& mesh_geometry, + mesh_msgs::msg::MeshMaterials& mesh_materials, + mesh_msgs::msg::MeshVertexColors& mesh_vertex_colors, + boost::optional&> texture_cache, std::string mesh_uuid ) { @@ -198,7 +198,7 @@ bool fromMeshBufferToMeshMessages( texture_cache.get().resize(n_textures); for (unsigned int i = 0; i < n_textures; i++) { - sensor_msgs::Image image; + sensor_msgs::msg::Image image; sensor_msgs::fillImage( image, "rgb8", @@ -207,7 +207,7 @@ bool fromMeshBufferToMeshMessages( buffer_textures[i].m_width * 3, // step size buffer_textures[i].m_data ); - mesh_msgs::MeshTexture texture; + mesh_msgs::msg::MeshTexture texture; texture.uuid = mesh_uuid; texture.texture_index = i; texture.image = image; @@ -220,22 +220,7 @@ bool fromMeshBufferToMeshMessages( } bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer) -{ - return fromMeshGeometryToMeshBuffer(*mesh_geometry_ptr, buffer); -} - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryConstPtr& mesh_geometry_ptr, - lvr2::MeshBufferPtr& buffer_ptr) -{ - if(!buffer_ptr) buffer_ptr = lvr2::MeshBufferPtr(new lvr2::MeshBuffer); - return fromMeshGeometryToMeshBuffer(*mesh_geometry_ptr, *buffer_ptr); -} - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, + const std::shared_ptr mesh_geometry_ptr, lvr2::MeshBufferPtr& buffer_ptr) { if(!buffer_ptr) buffer_ptr = lvr2::MeshBufferPtr(new lvr2::MeshBuffer); @@ -243,14 +228,7 @@ bool fromMeshGeometryToMeshBuffer( } bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometryPtr& mesh_geometry_ptr, - lvr2::MeshBuffer& buffer) -{ - return fromMeshGeometryToMeshBuffer(*mesh_geometry_ptr, buffer); -} - -bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBufferPtr& buffer_ptr) { if(!buffer_ptr) buffer_ptr = lvr2::MeshBufferPtr(new lvr2::MeshBuffer); @@ -258,7 +236,7 @@ bool fromMeshGeometryToMeshBuffer( } bool fromMeshGeometryToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, lvr2::MeshBuffer& buffer) { @@ -372,7 +350,9 @@ void removeDuplicates(lvr2::MeshBuffer& buffer) } */ -static inline bool hasCloudChannel(const sensor_msgs::PointCloud2& cloud, const std::string& field_name) +static inline bool hasCloudChannel( + const sensor_msgs::msg::PointCloud2& cloud, + const std::string& field_name) { // Get the index we need for (size_t d = 0; d < cloud.fields.size(); ++d) @@ -381,7 +361,9 @@ static inline bool hasCloudChannel(const sensor_msgs::PointCloud2& cloud, const return false; } -bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, lvr2::PointBuffer& buffer) +bool fromPointCloud2ToPointBuffer( + const sensor_msgs::msg::PointCloud2& cloud, + lvr2::PointBuffer& buffer) { size_t size = cloud.height * cloud.width; @@ -536,7 +518,7 @@ bool fromPointCloud2ToPointBuffer(const sensor_msgs::PointCloud2& cloud, lvr2::P } bool fromMeshGeometryMessageToMeshBuffer( - const mesh_msgs::MeshGeometry& mesh_geometry, + const mesh_msgs::msg::MeshGeometry& mesh_geometry, const lvr2::MeshBufferPtr& buffer ) { @@ -580,20 +562,26 @@ bool fromMeshGeometryMessageToMeshBuffer( } else { - ROS_ERROR_STREAM("Number of normals (" << mesh_geometry.vertex_normals.size() + // RCLCPP_ERROR_STREAM("Number of normals (" << mesh_geometry.vertex_normals.size() + // << ") must be equal to number of vertices (" << mesh_geometry.vertices.size() + // << "), ignore normals!"); + std::cerr << "Number of normals (" << mesh_geometry.vertex_normals.size() << ") must be equal to number of vertices (" << mesh_geometry.vertices.size() - << "), ignore normals!"); + << "), ignore normals!" << std::endl; } return true; } -void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string frame, sensor_msgs::PointCloud2Ptr& cloud) +void PointBufferToPointCloud2( + const lvr2::PointBufferPtr& buffer, + std::string frame, + std::shared_ptr& cloud) { // the offset will be updated by addPointField - cloud->header.stamp = ros::Time::now(); + cloud->header.stamp = rclcpp::Time(); cloud->header.frame_id = frame; - ros::Rate r(60); +// ros::Rate r(60); int type; std::map > floatChannels; @@ -613,9 +601,9 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr { size = channelPair.second.numElements(); int p_offset = 0; - p_offset = addPointField(*cloud, "x", 1, sensor_msgs::PointField::FLOAT32, p_offset); - p_offset = addPointField(*cloud, "y", 1, sensor_msgs::PointField::FLOAT32, p_offset); - p_offset = addPointField(*cloud, "z", 1, sensor_msgs::PointField::FLOAT32, p_offset); + p_offset = addPointField(*cloud, "x", 1, sensor_msgs::msg::PointField::FLOAT32, p_offset); + p_offset = addPointField(*cloud, "y", 1, sensor_msgs::msg::PointField::FLOAT32, p_offset); + p_offset = addPointField(*cloud, "z", 1, sensor_msgs::msg::PointField::FLOAT32, p_offset); p_offset += sizeof(float); cloud->point_step = offset; for(auto channelPair: uCharChannels) @@ -623,7 +611,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr if(channelPair.first == "colors") { - offset = addPointField(*cloud, "rgb", channelPair.second.width(), sensor_msgs::PointField::FLOAT32, offset); + offset = addPointField(*cloud, "rgb", channelPair.second.width(), sensor_msgs::msg::PointField::FLOAT32, offset); cloud->point_step = offset; } } @@ -668,7 +656,8 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr cloud->width = size; - ROS_INFO("Starting conversion."); +// RCLCPP_INFO("Starting conversion."); + std::cout << "Starting conversion." << std::endl; for(auto field: cloud->fields) { // Points is a special case... @@ -678,7 +667,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr //auto iter_x = floatIters.at("x"); //auto iter_y = floatIters.at("y"); //auto iter_z = floatIters.at("z"); - #pragma omp parallel for + for(size_t i = 0; i < size; ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]); @@ -693,7 +682,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr } else { - if(field.datatype == sensor_msgs::PointField::FLOAT32) + if(field.datatype == sensor_msgs::msg::PointField::FLOAT32) { std::string name = field.name; if(name == "rgb") @@ -701,7 +690,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr name = "colors"; auto channel = uCharChannels.at(name); //auto iter = uCharIters.at(field.name); -#pragma omp parallel for + for(size_t i = 0; i < size; ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; @@ -720,7 +709,7 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr // ELSE auto channel = floatChannels.at(field.name); //auto iter = floatIters.at(field.name); - #pragma omp parallel for + for(size_t i = 0; i < size; ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; @@ -731,21 +720,23 @@ void PointBufferToPointCloud2(const lvr2::PointBufferPtr& buffer, std::string fr } } } - else if (field.datatype == sensor_msgs::PointField::UINT8) + else if (field.datatype == sensor_msgs::msg::PointField::UINT8) { } } } - ROS_INFO("DONE"); +// RCLCPP_INFO("DONE"); } -void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::PointBufferPtr& buffer) +void PointCloud2ToPointBuffer( + const std::shared_ptr cloud, + lvr2::PointBufferPtr& buffer) { buffer = lvr2::PointBufferPtr(new lvr2::PointBuffer()); for(auto field : cloud->fields) { - if(field.datatype == sensor_msgs::PointField::FLOAT32) + if(field.datatype == sensor_msgs::msg::PointField::FLOAT32) { if(field.name == "x" || field.name == "y" || field.name == "z") { @@ -778,7 +769,6 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po buffer->setPointArray(points, cloud->width * cloud->height); channel = buffer->getChannel("points"); - #pragma omp parallel for for(size_t i = 0; i < (cloud->width * cloud->height); ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]); @@ -790,17 +780,17 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po } else { - if(field.datatype == sensor_msgs::PointField::FLOAT32) + if(field.datatype == sensor_msgs::msg::PointField::FLOAT32) { auto channel = buffer->getChannel(field.name); if(!channel) { - ROS_INFO("Channel %s missing", field.name.c_str()); + std::cout << "Channel " << field.name << " missing" << std::endl; + // RCLCPP_INFO("Channel %s missing", field.name.c_str()); continue; } - #pragma omp parallel for for(size_t i = 0; i < (cloud->width * cloud->height); ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; @@ -810,17 +800,17 @@ void PointCloud2ToPointBuffer(const sensor_msgs::PointCloud2Ptr& cloud, lvr2::Po } } } - else if (field.datatype == sensor_msgs::PointField::UINT8) + else if (field.datatype == sensor_msgs::msg::PointField::UINT8) { auto channel = buffer->getChannel(field.name); if(!channel) { - ROS_INFO("Channel %s missing", field.name.c_str()); + // RCLCPP_INFO("Channel %s missing", field.name.c_str()); + std::cout << "Channel " << field.name << " missing" << std::endl; continue; } - #pragma omp parallel for for(size_t i = 0; i < (cloud->width * cloud->height); ++i) { unsigned char* ptr = &(cloud->data[cloud->point_step * i]) + field.offset; diff --git a/mesh_msgs_hdf5/CMakeLists.txt b/mesh_msgs_hdf5/CMakeLists.txt index 60b34ff..8473dda 100644 --- a/mesh_msgs_hdf5/CMakeLists.txt +++ b/mesh_msgs_hdf5/CMakeLists.txt @@ -1,27 +1,32 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.8) project(mesh_msgs_hdf5) -set(PACKAGE_DEPENDENCIES - mesh_msgs - hdf5_map_io - label_manager -) -find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) -find_package(HDF5 REQUIRED COMPONENTS C CXX HL) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -### compile with c++14 -set(CMAKE_CXX_STANDARD 14) -catkin_package( - CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} - DEPENDS HDF5 -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(actionlib_msgs REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(hdf5_map_io REQUIRED) +find_package(label_manager REQUIRED) + +find_package(HDF5 REQUIRED COMPONENTS C CXX HL) + include_directories( include - ${catkin_INCLUDE_DIRS} ${HDF5_INCLUDE_DIRS} ) @@ -30,23 +35,31 @@ add_executable(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES} ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + hdf5_map_io + label_manager) + + install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include ) + +ament_package() diff --git a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h index f6c608e..cc5a1b0 100644 --- a/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h +++ b/mesh_msgs_hdf5/include/mesh_msgs_hdf5/mesh_msgs_hdf5.h @@ -1,34 +1,37 @@ #ifndef MESH_MSGS_HDF5_H_ #define MESH_MSGS_HDF5_H_ -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include +#include + +#include + +#include "mesh_msgs/msg/mesh_face_cluster_stamped.hpp" +#include "mesh_msgs/srv/get_geometry.hpp" +#include "mesh_msgs/srv/get_materials.hpp" +#include "mesh_msgs/srv/get_texture.hpp" +#include "mesh_msgs/srv/get_uui_ds.hpp" // :D GetUUIDs +#include "mesh_msgs/srv/get_vertex_colors.hpp" +#include "mesh_msgs/srv/get_vertex_costs.hpp" +#include "mesh_msgs/srv/get_vertex_cost_layers.hpp" +#include "mesh_msgs/srv/get_labeled_clusters.hpp" +#include "label_manager/srv/get_label_groups.hpp" +#include "label_manager/srv/get_labeled_cluster_group.hpp" +#include "label_manager/srv/delete_label.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/fill_image.hpp" + +#include "rclcpp/rclcpp.hpp" namespace mesh_msgs_hdf5 { -class hdf5_to_msg +class hdf5_to_msg : public rclcpp::Node { public: @@ -37,84 +40,81 @@ class hdf5_to_msg protected: void loadAndPublishGeometry(); - bool getVertices(std::vector& vertices, mesh_msgs::MeshGeometryStamped& geometryMsg); - bool getFaces(std::vector& faceIds, mesh_msgs::MeshGeometryStamped& geometryMsg); - bool getVertexNormals(std::vector& vertexNormals, mesh_msgs::MeshGeometryStamped& geometryMsg); + bool getVertices(std::vector& vertices, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); + bool getFaces(std::vector& faceIds, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); + bool getVertexNormals(std::vector& vertexNormals, mesh_msgs::msg::MeshGeometryStamped& geometryMsg); - bool getVertexColors(std::vector& vertexColors, mesh_msgs::MeshVertexColorsStamped& vertexColorsMsg); - bool getVertexCosts(std::vector& vertexCosts, std::string layer, mesh_msgs::MeshVertexCostsStamped& vertexCostsMsg); + bool getVertexColors(std::vector& vertexColors, mesh_msgs::msg::MeshVertexColorsStamped& vertexColorsMsg); + bool getVertexCosts(std::vector& vertexCosts, std::string layer, mesh_msgs::msg::MeshVertexCostsStamped& vertexCostsMsg); // Mesh services bool service_getUUIDs( - mesh_msgs::GetUUIDs::Request &req, - mesh_msgs::GetUUIDs::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometry( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometryVertices( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometryFaces( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getGeometryVertexNormals( - mesh_msgs::GetGeometry::Request &req, - mesh_msgs::GetGeometry::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getMaterials( - mesh_msgs::GetMaterials::Request &req, - mesh_msgs::GetMaterials::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getTexture( - mesh_msgs::GetTexture::Request &req, - mesh_msgs::GetTexture::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getVertexColors( - mesh_msgs::GetVertexColors::Request &req, - mesh_msgs::GetVertexColors::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getVertexCosts( - mesh_msgs::GetVertexCosts::Request &req, - mesh_msgs::GetVertexCosts::Response &res); + const std::shared_ptr req, + std::shared_ptr res); bool service_getVertexCostLayers( - mesh_msgs::GetVertexCostLayers::Request &req, - mesh_msgs::GetVertexCostLayers::Response &res); + const std::shared_ptr req, + std::shared_ptr res); // Label manager services bool service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request &req, - mesh_msgs::GetLabeledClusters::Response &res); + const std::shared_ptr req, + std::shared_ptr res); - void callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped::ConstPtr &msg); + void callback_clusterLabel(const mesh_msgs::msg::MeshFaceClusterStamped& msg); private: // Mesh message service servers - ros::ServiceServer srv_get_uuids_; - ros::ServiceServer srv_get_geometry_; - ros::ServiceServer srv_get_geometry_vertices_; - ros::ServiceServer srv_get_geometry_faces_; - ros::ServiceServer srv_get_geometry_vertex_normals_; - ros::ServiceServer srv_get_materials_; - ros::ServiceServer srv_get_texture_; - ros::ServiceServer srv_get_vertex_colors_; - ros::ServiceServer srv_get_vertex_costs_; - ros::ServiceServer srv_get_vertex_cost_layers_; + + rclcpp::Service::SharedPtr srv_get_uuids_; + rclcpp::Service::SharedPtr srv_get_geometry_; + rclcpp::Service::SharedPtr srv_get_geometry_vertices_; + rclcpp::Service::SharedPtr srv_get_geometry_faces_; + rclcpp::Service::SharedPtr srv_get_geometry_vertex_normals_; + rclcpp::Service::SharedPtr srv_get_materials_; + rclcpp::Service::SharedPtr srv_get_texture_; + rclcpp::Service::SharedPtr srv_get_vertex_colors_; + rclcpp::Service::SharedPtr srv_get_vertex_costs_; + rclcpp::Service::SharedPtr srv_get_vertex_cost_layers_; // Mesh message publishers - ros::Publisher pub_geometry_; - ros::Publisher pub_vertex_colors_; - ros::Publisher pub_vertex_costs_; + rclcpp::Publisher::SharedPtr pub_geometry_; + rclcpp::Publisher::SharedPtr pub_vertex_colors_; + rclcpp::Publisher::SharedPtr pub_vertex_costs_; // Label manager services and subs/pubs - ros::ServiceServer srv_get_labeled_clusters_; - ros::Subscriber sub_cluster_label_; - - // ROS - ros::NodeHandle node_handle; + rclcpp::Service::SharedPtr srv_get_labeled_clusters_; + rclcpp::Subscription::SharedPtr sub_cluster_label_; // ROS parameter std::string inputFile; std::string mesh_uuid = "mesh"; - }; } // end namespace diff --git a/mesh_msgs_hdf5/package.xml b/mesh_msgs_hdf5/package.xml index d3caf7f..b69cd08 100644 --- a/mesh_msgs_hdf5/package.xml +++ b/mesh_msgs_hdf5/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs_hdf5 1.1.0 Read mesh_msgs from hdf5 @@ -8,14 +9,19 @@ BSD-3 Sebastian Pütz - mesh_msgs - hdf5_map_io - label_manager + ament_cmake - mesh_msgs - hdf5_map_io - label_manager + rclcpp + rclcpp_action + rclcpp_components + actionlib_msgs + mesh_msgs + hdf5_map_io + label_manager - catkin + ament_lint_common + + ament_cmake + diff --git a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp index 98ace78..35347e0 100644 --- a/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp +++ b/mesh_msgs_hdf5/src/mesh_msgs_hdf5.cpp @@ -1,49 +1,60 @@ #include #include +#include -namespace mesh_msgs_hdf5 { +#include "rclcpp/executors.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" -hdf5_to_msg::hdf5_to_msg() +using std::placeholders::_1; +using std::placeholders::_2; + +namespace mesh_msgs_hdf5 { - ros::NodeHandle nh("~"); - if (!nh.getParam("inputFile", inputFile)) - { - inputFile = "/tmp/map.h5"; - } +hdf5_to_msg::hdf5_to_msg() +:rclcpp::Node("mesh_msgs_hdf5") +{ + // TODO: check if this is correct + this->declare_parameter("inputFile", "/tmp/map.h5"); + inputFile = this->get_parameter("inputFile").as_string(); + + RCLCPP_INFO_STREAM(this->get_logger(), "Using input file: " << inputFile); + + srv_get_uuids_ = this->create_service( + "get_uuids", std::bind(&hdf5_to_msg::service_getUUIDs, this, _1, _2)); + srv_get_geometry_ = this->create_service( + "get_geometry", std::bind(&hdf5_to_msg::service_getGeometry, this, _1, _2)); + srv_get_geometry_vertices_ = this->create_service( + "get_geometry_vertices", std::bind(&hdf5_to_msg::service_getGeometryVertices, this, _1, _2)); + srv_get_geometry_faces_ = this->create_service( + "get_geometry_faces", std::bind(&hdf5_to_msg::service_getGeometryFaces, this, _1, _2)); + srv_get_geometry_vertex_normals_ = this->create_service( + "get_geometry_vertexnormals", std::bind(&hdf5_to_msg::service_getGeometryVertexNormals, this, _1, _2)); + srv_get_materials_ = this->create_service( + "get_materials", std::bind(&hdf5_to_msg::service_getMaterials, this, _1, _2)); + srv_get_texture_ = this->create_service( + "get_texture", std::bind(&hdf5_to_msg::service_getTexture, this, _1, _2)); + srv_get_vertex_colors_ = this->create_service( + "get_vertex_colors", std::bind(&hdf5_to_msg::service_getVertexColors, this, _1, _2)); + srv_get_vertex_costs_ = this->create_service( + "get_vertex_costs", std::bind(&hdf5_to_msg::service_getVertexCosts, this, _1, _2)); + srv_get_vertex_cost_layers_ = this->create_service( + "get_vertex_cost_layers", std::bind(&hdf5_to_msg::service_getVertexCostLayers, this, _1, _2)); + + pub_geometry_ = this->create_publisher( + "mesh/geometry", 1); + pub_vertex_colors_ = this->create_publisher( + "mesh/vertex_colors", 1); + pub_vertex_costs_ = this->create_publisher( + "mesh/vertex_costs", 1); + + + srv_get_labeled_clusters_ = this->create_service( + "get_labeled_clusters", std::bind(&hdf5_to_msg::service_getLabeledClusters, this, _1, _2)); + + sub_cluster_label_ = this->create_subscription( + "cluster_label", 10, std::bind(&hdf5_to_msg::callback_clusterLabel, this, _1)); - ROS_INFO_STREAM("Using input file: " << inputFile); - - srv_get_geometry_ = node_handle.advertiseService( - "get_geometry", &hdf5_to_msg::service_getGeometry, this); - srv_get_geometry_vertices_ = node_handle.advertiseService( - "get_geometry_vertices", &hdf5_to_msg::service_getGeometryVertices, this); - srv_get_geometry_faces_ = node_handle.advertiseService( - "get_geometry_faces", &hdf5_to_msg::service_getGeometryFaces, this); - srv_get_geometry_vertex_normals_ = node_handle.advertiseService( - "get_geometry_vertexnormals", &hdf5_to_msg::service_getGeometryVertexNormals, this); - srv_get_materials_ = node_handle.advertiseService( - "get_materials", &hdf5_to_msg::service_getMaterials, this); - srv_get_texture_ = node_handle.advertiseService( - "get_texture", &hdf5_to_msg::service_getTexture, this); - srv_get_uuids_ = node_handle.advertiseService( - "get_uuids", &hdf5_to_msg::service_getUUIDs, this); - srv_get_vertex_colors_ = node_handle.advertiseService( - "get_vertex_colors", &hdf5_to_msg::service_getVertexColors, this); - srv_get_vertex_costs_ = node_handle.advertiseService( - "get_vertex_costs", &hdf5_to_msg::service_getVertexCosts, this); - srv_get_vertex_cost_layers_ = node_handle.advertiseService( - "get_vertex_cost_layers", &hdf5_to_msg::service_getVertexCostLayers, this); - - pub_geometry_ = node_handle.advertise("mesh/geometry", 1, true); - pub_vertex_colors_ = node_handle.advertise("mesh/vertex_colors", 1, true); - pub_vertex_costs_ = node_handle.advertise("mesh/vertex_costs", 1); - - - srv_get_labeled_clusters_ = node_handle.advertiseService( - "get_labeled_clusters", &hdf5_to_msg::service_getLabeledClusters, this); - - sub_cluster_label_ = node_handle.subscribe("cluster_label", 10, &hdf5_to_msg::callback_clusterLabel, this); loadAndPublishGeometry(); } @@ -53,7 +64,7 @@ void hdf5_to_msg::loadAndPublishGeometry() hdf5_map_io::HDF5MapIO io(inputFile); // geometry - mesh_msgs::MeshGeometryStamped geometryMsg; + mesh_msgs::msg::MeshGeometryStamped geometryMsg; auto vertices = io.getVertices(); auto faceIds = io.getFaceIds(); @@ -63,18 +74,18 @@ void hdf5_to_msg::loadAndPublishGeometry() getFaces(faceIds, geometryMsg); getVertexNormals(vertexNormals, geometryMsg); - pub_geometry_.publish(geometryMsg); + pub_geometry_->publish(geometryMsg); // vertex colors - mesh_msgs::MeshVertexColorsStamped vertexColorsMsg; + mesh_msgs::msg::MeshVertexColorsStamped vertexColorsMsg; auto vertexColors = io.getVertexColors(); getVertexColors(vertexColors, vertexColorsMsg); - pub_vertex_colors_.publish(vertexColorsMsg); + pub_vertex_colors_->publish(vertexColorsMsg); // vertex costs - mesh_msgs::MeshVertexCostsStamped vertexCostsMsg; + mesh_msgs::msg::MeshVertexCostsStamped vertexCostsMsg; for (std::string costlayer : io.getCostLayers()) { try @@ -82,19 +93,21 @@ void hdf5_to_msg::loadAndPublishGeometry() auto costs = io.getVertexCosts(costlayer); getVertexCosts(costs, costlayer, vertexCostsMsg); - pub_vertex_costs_.publish(vertexCostsMsg); + pub_vertex_costs_->publish(vertexCostsMsg); } catch (const hf::DataSpaceException& e) { - ROS_WARN_STREAM("Could not load costlayer " << costlayer); + RCLCPP_WARN_STREAM(this->get_logger(), "Could not load costlayer " << costlayer); } } } -bool hdf5_to_msg::getVertices(std::vector& vertices, mesh_msgs::MeshGeometryStamped& geometryMsg) +bool hdf5_to_msg::getVertices( + std::vector& vertices, + mesh_msgs::msg::MeshGeometryStamped& geometryMsg) { unsigned int nVertices = vertices.size() / 3; - ROS_INFO_STREAM("Found " << nVertices << " vertices"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertices << " vertices"); geometryMsg.mesh_geometry.vertices.resize(nVertices); for (unsigned int i = 0; i < nVertices; i++) { @@ -106,15 +119,17 @@ bool hdf5_to_msg::getVertices(std::vector& vertices, mesh_msgs::MeshGeome // Header geometryMsg.uuid = mesh_uuid; geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = ros::Time::now(); + geometryMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getFaces(std::vector& faceIds, mesh_msgs::MeshGeometryStamped& geometryMsg) +bool hdf5_to_msg::getFaces( + std::vector& faceIds, + mesh_msgs::msg::MeshGeometryStamped& geometryMsg) { unsigned int nFaces = faceIds.size() / 3; - ROS_INFO_STREAM("Found " << nFaces << " faces"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nFaces << " faces"); geometryMsg.mesh_geometry.faces.resize(nFaces); for (unsigned int i = 0; i < nFaces; i++) { @@ -126,15 +141,17 @@ bool hdf5_to_msg::getFaces(std::vector& faceIds, mesh_msgs::MeshGeomet // Header geometryMsg.uuid = mesh_uuid; geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = ros::Time::now(); + geometryMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getVertexNormals(std::vector& vertexNormals, mesh_msgs::MeshGeometryStamped& geometryMsg) +bool hdf5_to_msg::getVertexNormals( + std::vector& vertexNormals, + mesh_msgs::msg::MeshGeometryStamped& geometryMsg) { unsigned int nVertexNormals = vertexNormals.size() / 3; - ROS_INFO_STREAM("Found " << nVertexNormals << " vertex normals"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertexNormals << " vertex normals"); geometryMsg.mesh_geometry.vertex_normals.resize(nVertexNormals); for (unsigned int i = 0; i < nVertexNormals; i++) { @@ -146,15 +163,17 @@ bool hdf5_to_msg::getVertexNormals(std::vector& vertexNormals, mesh_msgs: // Header geometryMsg.uuid = mesh_uuid; geometryMsg.header.frame_id = "map"; - geometryMsg.header.stamp = ros::Time::now(); + geometryMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getVertexColors(std::vector& vertexColors, mesh_msgs::MeshVertexColorsStamped& vertexColorsMsg) +bool hdf5_to_msg::getVertexColors( + std::vector& vertexColors, + mesh_msgs::msg::MeshVertexColorsStamped& vertexColorsMsg) { unsigned int nVertices = vertexColors.size() / 3; - ROS_INFO_STREAM("Found " << nVertices << " vertices for vertex colors"); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nVertices << " vertices for vertex colors"); vertexColorsMsg.mesh_vertex_colors.vertex_colors.resize(nVertices); for (unsigned int i = 0; i < nVertices; i++) { @@ -171,12 +190,15 @@ bool hdf5_to_msg::getVertexColors(std::vector& vertexColors, mesh_msgs: // Header vertexColorsMsg.uuid = mesh_uuid; vertexColorsMsg.header.frame_id = "map"; - vertexColorsMsg.header.stamp = ros::Time::now(); + vertexColorsMsg.header.stamp = this->get_clock()->now(); return true; } -bool hdf5_to_msg::getVertexCosts(std::vector& costs, std::string layer, mesh_msgs::MeshVertexCostsStamped& vertexCostsMsg) +bool hdf5_to_msg::getVertexCosts( + std::vector& costs, + std::string layer, + mesh_msgs::msg::MeshVertexCostsStamped& vertexCostsMsg) { vertexCostsMsg.mesh_vertex_costs.costs.resize(costs.size()); for (uint32_t i = 0; i < costs.size(); i++) @@ -187,76 +209,76 @@ bool hdf5_to_msg::getVertexCosts(std::vector& costs, std::string layer, m vertexCostsMsg.uuid = mesh_uuid; vertexCostsMsg.type = layer; vertexCostsMsg.header.frame_id = "map"; - vertexCostsMsg.header.stamp = ros::Time::now(); + vertexCostsMsg.header.stamp = this->get_clock()->now(); return true; } bool hdf5_to_msg::service_getUUIDs( - mesh_msgs::GetUUIDs::Request& req, - mesh_msgs::GetUUIDs::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { - res.uuids.push_back(mesh_uuid); + res->uuids.push_back(mesh_uuid); return true; } bool hdf5_to_msg::service_getGeometry( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertices auto vertices = io.getVertices(); - getVertices(vertices, res.mesh_geometry_stamped); + getVertices(vertices, res->mesh_geometry_stamped); // Faces auto faceIds = io.getFaceIds(); - getFaces(faceIds, res.mesh_geometry_stamped); + getFaces(faceIds, res->mesh_geometry_stamped); // Vertex normals auto vertexNormals = io.getVertexNormals(); - getVertexNormals(vertexNormals, res.mesh_geometry_stamped); + getVertexNormals(vertexNormals, res->mesh_geometry_stamped); return true; } bool hdf5_to_msg::service_getGeometryVertices( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertices auto vertices = io.getVertices(); - return getVertices(vertices, res.mesh_geometry_stamped); + return getVertices(vertices, res->mesh_geometry_stamped); } bool hdf5_to_msg::service_getGeometryFaces( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Faces auto faceIds = io.getFaceIds(); - return getFaces(faceIds, res.mesh_geometry_stamped); + return getFaces(faceIds, res->mesh_geometry_stamped); } bool hdf5_to_msg::service_getGeometryVertexNormals( - mesh_msgs::GetGeometry::Request& req, - mesh_msgs::GetGeometry::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertex normals auto vertexNormals = io.getVertexNormals(); - return getVertexNormals(vertexNormals, res.mesh_geometry_stamped); + return getVertexNormals(vertexNormals, res->mesh_geometry_stamped); } bool hdf5_to_msg::service_getMaterials( - mesh_msgs::GetMaterials::Request& req, - mesh_msgs::GetMaterials::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); @@ -266,29 +288,29 @@ bool hdf5_to_msg::service_getMaterials( auto materialFaceIndices = io.getMaterialFaceIndices(); // for each face: material index unsigned int nMaterials = materials.size(); unsigned int nFaces = materialFaceIndices.size(); - ROS_INFO_STREAM("Found " << nMaterials << " materials and " << nFaces << " faces"); - res.mesh_materials_stamped.mesh_materials.materials.resize(nMaterials); + RCLCPP_INFO_STREAM(this->get_logger(), "Found " << nMaterials << " materials and " << nFaces << " faces"); + res->mesh_materials_stamped.mesh_materials.materials.resize(nMaterials); for (uint32_t i = 0; i < nMaterials; i++) { int texture_index = materials[i].textureIndex; // has texture - res.mesh_materials_stamped + res->mesh_materials_stamped .mesh_materials .materials[i] .has_texture = texture_index >= 0; // texture index - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .texture_index = static_cast(texture_index); // color - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.r = materials[i].r / 255.0f; - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.g = materials[i].g / 255.0f; - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.b = materials[i].b / 255.0f; - res.mesh_materials_stamped.mesh_materials.materials[i] + res->mesh_materials_stamped.mesh_materials.materials[i] .color.a = 1; } @@ -309,53 +331,53 @@ bool hdf5_to_msg::service_getMaterials( materialToFaces[materialIndex].push_back(i); } // For each material, map contains a list of faces - res.mesh_materials_stamped.mesh_materials.clusters.resize(nMaterials); + res->mesh_materials_stamped.mesh_materials.clusters.resize(nMaterials); for (uint32_t i = 0; i < nMaterials; i++) { for (uint32_t j = 0; j < materialToFaces[i].size(); j++) { - res.mesh_materials_stamped.mesh_materials.clusters[i].face_indices.push_back(materialToFaces[i][j]); + res->mesh_materials_stamped.mesh_materials.clusters[i].face_indices.push_back(materialToFaces[i][j]); } } - res.mesh_materials_stamped.mesh_materials.cluster_materials.resize(nMaterials); + res->mesh_materials_stamped.mesh_materials.cluster_materials.resize(nMaterials); for (uint32_t i = 0; i < nMaterials; i++) { - res.mesh_materials_stamped.mesh_materials.cluster_materials[i] = i; + res->mesh_materials_stamped.mesh_materials.cluster_materials[i] = i; } // Vertex Tex Coords auto vertexTexCoords = io.getVertexTextureCoords(); unsigned int nVertices = vertexTexCoords.size() / 3; - res.mesh_materials_stamped.mesh_materials.vertex_tex_coords.resize(nVertices); + res->mesh_materials_stamped.mesh_materials.vertex_tex_coords.resize(nVertices); for (uint32_t i = 0; i < nVertices; i++) { // coords: u/v/w // w is always 0 - res.mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].u = vertexTexCoords[3 * i]; - res.mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].v = vertexTexCoords[3 * i + 1]; + res->mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].u = vertexTexCoords[3 * i]; + res->mesh_materials_stamped.mesh_materials.vertex_tex_coords[i].v = vertexTexCoords[3 * i + 1]; } // Header - res.mesh_materials_stamped.uuid = mesh_uuid; - res.mesh_materials_stamped.header.frame_id = "map"; - res.mesh_materials_stamped.header.stamp = ros::Time::now(); + res->mesh_materials_stamped.uuid = mesh_uuid; + res->mesh_materials_stamped.header.frame_id = "map"; + res->mesh_materials_stamped.header.stamp = this->get_clock()->now(); return true; } bool hdf5_to_msg::service_getTexture( - mesh_msgs::GetTexture::Request& req, - mesh_msgs::GetTexture::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); for (auto texture : io.getTextures()) { - if (std::stoi(texture.name) == req.texture_index) + if (std::stoi(texture.name) == req->texture_index) { - res.texture.texture_index = req.texture_index; - res.texture.uuid = mesh_uuid; - sensor_msgs::Image image; + res->texture.texture_index = req->texture_index; + res->texture.uuid = mesh_uuid; + sensor_msgs::msg::Image image; sensor_msgs::fillImage( // TODO: only RGB, breaks when using other color channels image, "rgb8", @@ -365,7 +387,7 @@ bool hdf5_to_msg::service_getTexture( texture.data.data() ); - res.texture.image = image; + res->texture.image = image; return true; } @@ -375,39 +397,39 @@ bool hdf5_to_msg::service_getTexture( } bool hdf5_to_msg::service_getVertexColors( - mesh_msgs::GetVertexColors::Request& req, - mesh_msgs::GetVertexColors::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); // Vertex colors auto vertexColors = io.getVertexColors(); - return getVertexColors(vertexColors, res.mesh_vertex_colors_stamped); + return getVertexColors(vertexColors, res->mesh_vertex_colors_stamped); } bool hdf5_to_msg::service_getVertexCosts( - mesh_msgs::GetVertexCosts::Request& req, - mesh_msgs::GetVertexCosts::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); - auto costs = io.getVertexCosts(req.layer); - return getVertexCosts(costs, req.layer, res.mesh_vertex_costs_stamped); + auto costs = io.getVertexCosts(req->layer); + return getVertexCosts(costs, req->layer, res->mesh_vertex_costs_stamped); } bool hdf5_to_msg::service_getVertexCostLayers( - mesh_msgs::GetVertexCostLayers::Request &req, - mesh_msgs::GetVertexCostLayers::Response &res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); - res.layers = io.getCostLayers(); + res->layers = io.getCostLayers(); return true; } bool hdf5_to_msg::service_getLabeledClusters( - mesh_msgs::GetLabeledClusters::Request& req, - mesh_msgs::GetLabeledClusters::Response& res) + const std::shared_ptr req, + std::shared_ptr res) { hdf5_map_io::HDF5MapIO io(inputFile); @@ -421,7 +443,7 @@ bool hdf5_to_msg::service_getLabeledClusters( { // copy label auto faceIds = io.getFaceIdsOfLabel(groups[i], labelsInGroup[j]); - mesh_msgs::MeshFaceCluster cluster; + mesh_msgs::msg::MeshFaceCluster cluster; std::stringstream ss; ss << groups[i] << "_" << labelsInGroup[j]; cluster.label = ss.str(); @@ -430,33 +452,34 @@ bool hdf5_to_msg::service_getLabeledClusters( { cluster.face_indices[k] = faceIds[k]; } - res.clusters.push_back(cluster); + res->clusters.push_back(cluster); } } return true; } -void hdf5_to_msg::callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped::ConstPtr& msg) +void hdf5_to_msg::callback_clusterLabel( + const mesh_msgs::msg::MeshFaceClusterStamped& msg) { - if (msg->uuid.compare(mesh_uuid) != 0) + if (msg.uuid.compare(mesh_uuid) != 0) { - ROS_ERROR("Invalid mesh UUID"); + RCLCPP_ERROR(this->get_logger(), "Invalid mesh UUID"); return; } hdf5_map_io::HDF5MapIO io(inputFile); // TODO: implement optional override - ROS_WARN("Override is enabled by default"); + RCLCPP_WARN(this->get_logger(), "Override is enabled by default"); // split label id into group and name std::vector split_results; - boost::split(split_results, msg->cluster.label, [](char c){ return c == '_'; }); + boost::split(split_results, msg.cluster.label, [](char c){ return c == '_'; }); if (split_results.size() != 2) { - ROS_ERROR("Received illegal cluster name"); + RCLCPP_ERROR(this->get_logger(), "Received illegal cluster name"); return; } @@ -464,9 +487,9 @@ void hdf5_to_msg::callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped: std::string label_group = split_results[0]; std::string label_name = split_results[1]; std::vector indices; - for (size_t i = 0; i < msg->cluster.face_indices.size(); i++) + for (size_t i = 0; i < msg.cluster.face_indices.size(); i++) { - indices.push_back(msg->cluster.face_indices[i]); + indices.push_back(msg.cluster.face_indices[i]); } // write to hdf5 @@ -475,11 +498,12 @@ void hdf5_to_msg::callback_clusterLabel(const mesh_msgs::MeshFaceClusterStamped: } // namespace mesh_msgs_hdf5 -int main(int argc, char **args) +int main(int argc, char **argv) { - ros::init(argc, args, "mesh_msgs_hdf5"); - mesh_msgs_hdf5::hdf5_to_msg hdf5_to_msg; - ros::MultiThreadedSpinner spinner(4); - spinner.spin(); + rclcpp::init(argc, argv); + rclcpp::ExecutorOptions opts; + rclcpp::executors::MultiThreadedExecutor executor(opts, 4); + executor.add_node(std::make_shared()); + executor.spin(); return 0; } diff --git a/mesh_msgs_transform/CMakeLists.txt b/mesh_msgs_transform/CMakeLists.txt index a3aa19a..5dc90b8 100644 --- a/mesh_msgs_transform/CMakeLists.txt +++ b/mesh_msgs_transform/CMakeLists.txt @@ -1,40 +1,77 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(mesh_msgs_transform) -find_package(catkin REQUIRED COMPONENTS - tf - geometry_msgs - mesh_msgs -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) -catkin_package( - INCLUDE_DIRS include - LIBRARIES mesh_msgs_transform - CATKIN_DEPENDS tf geometry_msgs mesh_msgs +add_library(${PROJECT_NAME} + src/transforms.cpp ) -add_library(mesh_msgs_transform - src/transforms.cpp +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" + ${EIGEN3_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + Eigen3::Eigen ) -target_link_libraries(mesh_msgs_transform - ${catkin_LIBRARIES} +target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_MSGS_TRANSFORM_BUILDING_LIBRARY")# + +ament_target_dependencies(${PROJECT_NAME} PUBLIC + rclcpp + mesh_msgs + tf2 + tf2_ros + geometry_msgs ) -add_dependencies(mesh_msgs_transform ${catkin_EXPORTED_TARGETS}) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -install(TARGETS mesh_msgs_transform - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +ament_export_dependencies( + rclcpp + mesh_msgs + tf2 + tf2_ros + geometry_msgs ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/mesh_msgs_transform/README.md b/mesh_msgs_transform/README.md new file mode 100644 index 0000000..6adaf25 --- /dev/null +++ b/mesh_msgs_transform/README.md @@ -0,0 +1,7 @@ +# mesh_msgs_transform + +Functions for transforming meshes. + +## ROS2 Port TODOs + +Here is still tf(1) used. Is this package in use? \ No newline at end of file diff --git a/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h b/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h index 8f6c66f..5c4a507 100644 --- a/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h +++ b/mesh_msgs_transform/include/mesh_msgs_transform/transforms.h @@ -46,17 +46,20 @@ #ifndef MESH_MSGS_TRANSFORM__TRANSFORMS_H_ #define MESH_MSGS_TRANSFORM__TRANSFORMS_H_ -#include -#include +#include +#include -namespace mesh_msgs_transform{ - bool transformGeometryMeshNoTime( - const std::string& target_frame, - const mesh_msgs::MeshGeometryStamped& mesh_in, - const std::string& fixed_frame, - mesh_msgs::MeshGeometryStamped& mesh_out, - const tf::TransformListener& tf_listener - ); -} +namespace mesh_msgs_transform +{ -#endif /* transforms.h */ +bool transformGeometryMeshNoTime( + const std::string& target_frame, + const mesh_msgs::msg::MeshGeometryStamped& mesh_in, + const std::string& fixed_frame, + mesh_msgs::msg::MeshGeometryStamped& mesh_out, + const tf2_ros::Buffer& tf_buffer +); + +} // namespace mesh_msgs_transform + +#endif // MESH_MSGS_TRANSFORM__TRANSFORMS_H_ diff --git a/mesh_msgs_transform/package.xml b/mesh_msgs_transform/package.xml index ad5ff94..d7fca0c 100644 --- a/mesh_msgs_transform/package.xml +++ b/mesh_msgs_transform/package.xml @@ -1,5 +1,6 @@ - + + mesh_msgs_transform 1.1.0 Methods to transform mesh_msgs @@ -8,16 +9,18 @@ http://wiki.ros.org/ros_mesh_tools/mesh_msgs_transform Sebastian Pütz - tf - mesh_msgs - geometry_msgs - eigen + ament_cmake + rclcpp + tf2 + tf2_ros + mesh_msgs + geometry_msgs + eigen - tf - mesh_msgs - geometry_msgs - eigen - - catkin + ament_lint_auto + ament_lint_common + + ament_cmake + diff --git a/mesh_msgs_transform/src/transforms.cpp b/mesh_msgs_transform/src/transforms.cpp index b07c7db..726a5c7 100644 --- a/mesh_msgs_transform/src/transforms.cpp +++ b/mesh_msgs_transform/src/transforms.cpp @@ -42,26 +42,35 @@ * author: Sebastian Pütz */ - - #include "mesh_msgs_transform/transforms.h" -#include +#include +#include +#include namespace mesh_msgs_transform{ -inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec){ +inline void vectorTfToEigen( + const tf2::Vector3& tf_vec, + Eigen::Vector3d& eigen_vec) +{ eigen_vec(0) = tf_vec[0]; eigen_vec(1) = tf_vec[1]; eigen_vec(2) = tf_vec[2]; } -inline void pointMsgToEigen(const geometry_msgs::Point& gm_point, Eigen::Vector3d& eigen_point){ +inline void pointMsgToEigen( + const geometry_msgs::msg::Point& gm_point, + Eigen::Vector3d& eigen_point) +{ eigen_point(0) = gm_point.x; eigen_point(1) = gm_point.y; eigen_point(2) = gm_point.z; } -inline void pointEigenToMsg(const Eigen::Vector3d& eigen_point, geometry_msgs::Point& gm_point){ +inline void pointEigenToMsg( + const Eigen::Vector3d& eigen_point, + geometry_msgs::msg::Point& gm_point) +{ gm_point.x = eigen_point(0); gm_point.y = eigen_point(1); gm_point.z = eigen_point(2); @@ -69,48 +78,41 @@ inline void pointEigenToMsg(const Eigen::Vector3d& eigen_point, geometry_msgs::P bool transformGeometryMeshNoTime( const std::string& target_frame, - const mesh_msgs::MeshGeometryStamped& mesh_in, + const mesh_msgs::msg::MeshGeometryStamped& mesh_in, const std::string& fixed_frame, - mesh_msgs::MeshGeometryStamped& mesh_out, - const tf::TransformListener& tf_listener -) + mesh_msgs::msg::MeshGeometryStamped& mesh_out, + const tf2_ros::Buffer& tf_buffer) { - tf::StampedTransform transform; - try{ - tf_listener.lookupTransform ( - target_frame, - ros::Time(0), mesh_in.header.frame_id, - ros::Time(0), - fixed_frame, - transform - ); - } - catch (tf::LookupException &e){ - ROS_ERROR ("%s", e.what ()); - return false; + geometry_msgs::msg::TransformStamped transform; + try + { + transform = tf_buffer.lookupTransform( + target_frame, tf2::TimePointZero, + mesh_in.header.frame_id, tf2::TimePointZero, + fixed_frame); } - catch (tf::ExtrapolationException &e){ - ROS_ERROR ("%s", e.what ()); + catch(const tf2::TransformException& ex) + { + RCLCPP_ERROR(rclcpp::get_logger("mesh_msgs_transform"), "%s", ex.what()); return false; } - tf::Quaternion quaternion = transform.getRotation (); - Eigen::Quaterniond rotation ( - quaternion.w (), - quaternion.x (), - quaternion.y (), - quaternion.z () - ); + Eigen::Quaterniond rotation( + transform.transform.rotation.w, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z); + + Eigen::Translation3d translation( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z ); - tf::Vector3 origin = transform.getOrigin (); - Eigen::Vector3d eigen_origin; - vectorTfToEigen(origin, eigen_origin); - Eigen::Translation3d translation ( eigen_origin ); Eigen::Affine3d transformation = translation * rotation; - if (&mesh_in != &mesh_out){ + if(&mesh_in != &mesh_out) + { mesh_out.header = mesh_in.header; - mesh_out.header.stamp = ros::Time::now(); mesh_out.mesh_geometry.faces = mesh_in.mesh_geometry.faces; } @@ -136,7 +138,7 @@ bool transformGeometryMeshNoTime( } mesh_out.header.frame_id = target_frame; - mesh_out.header.stamp = ros::Time::now(); + mesh_out.header.stamp = mesh_in.header.stamp;; return true; } diff --git a/mesh_tools/CMakeLists.txt b/mesh_tools/CMakeLists.txt index 2111670..174d70b 100644 --- a/mesh_tools/CMakeLists.txt +++ b/mesh_tools/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mesh_tools) -find_package(catkin REQUIRED) -catkin_metapackage() +cmake_minimum_required(VERSION 3.8) +project(mesh_tools NONE) +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/mesh_tools/package.xml b/mesh_tools/package.xml index 652e2e0..894c281 100644 --- a/mesh_tools/package.xml +++ b/mesh_tools/package.xml @@ -1,5 +1,6 @@ - + + mesh_tools 1.1.0 The mesh_tools package @@ -8,15 +9,17 @@ http://wiki.ros.org/mesh_tools Sebastian Pütz + ament_cmake + hdf5_map_io - mesh_msgs - mesh_msgs_transform - mesh_msgs_conversions - rviz_map_plugin - label_manager - catkin + + + + + + - + ament_cmake diff --git a/rviz_map_plugin/CMakeLists.txt b/rviz_map_plugin/CMakeLists.txt deleted file mode 100644 index 3fc63da..0000000 --- a/rviz_map_plugin/CMakeLists.txt +++ /dev/null @@ -1,106 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(rviz_map_plugin) - -set(THIS_PACKAGE_ROS_DEPS - roscpp - rviz - std_msgs - mesh_msgs - hdf5_map_io -) - -find_package(catkin REQUIRED COMPONENTS - ${THIS_PACKAGE_ROS_DEPS} -) - -find_package(Boost REQUIRED COMPONENTS system) -find_package(HDF5 REQUIRED COMPONENTS C CXX HL) -find_package(OpenCL 2 REQUIRED) - -catkin_package( - CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} - DEPENDS Boost OpenCL HDF5 OpenCL -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${HDF5_INCLUDE_DIRS} - ${OpenCL_INCLUDE_DIRS} -) - -## This setting causes Qt's "MOC" generation to happen automatically. -set(CMAKE_AUTOMOC ON) - -if("${rviz_QT_VERSION}" VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - ## pull in all required include dirs, define QT_LIBRARIES, etc. - include(${QT_USE_FILE}) -else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies - set(QT_LIBRARIES Qt5::Widgets) -endif() - -add_definitions(-DQT_NO_KEYWORDS) - -set(SOURCE_FILES - src/ClusterLabelDisplay.cpp - src/ClusterLabelPanel.cpp - src/ClusterLabelTool.cpp - src/ClusterLabelVisual.cpp - src/MapDisplay.cpp - src/MeshDisplay.cpp - src/MeshVisual.cpp - src/RvizFileProperty.cpp - src/MeshPoseTool.cpp - src/MeshGoalTool.cpp -) - -set(HEADER_FILES - include/ClusterLabelDisplay.hpp - include/ClusterLabelPanel.hpp - include/ClusterLabelVisual.hpp - include/MapDisplay.hpp - include/MeshDisplay.hpp - include/MeshVisual.hpp - include/ClusterLabelTool.hpp - include/CLUtil.hpp - include/RvizFileProperty.hpp - include/MeshPoseTool.hpp - include/MeshGoalTool.hpp -) - -add_library(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES} ${HDF5_LIBRARIES} ${HDF5_HL_LIBRARIES}) - -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} - ${catkin_LIBRARIES} - ${HDF5_LIBRARIES} - ${HDF5_HL_LIBRARIES} - ${OpenCL_LIBRARIES} -) - -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(FILES - rviz_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY icons/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) - -get_property(dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES) -foreach(dir ${dirs}) - message(STATUS "dir='${dir}'") -endforeach() diff --git a/rviz_map_plugin/package.xml b/rviz_map_plugin/package.xml deleted file mode 100644 index 6959b4c..0000000 --- a/rviz_map_plugin/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - rviz_map_plugin - - RViz display types and tools for the mesh_msgs package. - - 1.1.0 - Sebastian Pütz - Sebastian Pütz - Kristin Schmidt - Jan Philipp Vogtherr - Malte kleine Piening - catkin - - BSD-3 - https://github.com/uos/mesh_tools/tree/master/rviz_map_plugin - qtbase5-dev - roscpp - rviz - std_msgs - mesh_msgs - hdf5_map_io - ocl-icd-opencl-dev - opencl-headers - libqt5-core - libqt5-gui - libqt5-widgets - roscpp - rviz - std_msgs - mesh_msgs - hdf5_map_io - ocl-icd-opencl-dev - opencl-headers - - - - diff --git a/rviz_map_plugin/rviz_plugin.xml b/rviz_map_plugin/rviz_plugin.xml deleted file mode 100644 index 455f46c..0000000 --- a/rviz_map_plugin/rviz_plugin.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - A panel widget allowing creation of cluster labels. - - - - - A tool allowing selection of clusters. - - - - - Displays labeled clusters of a map. (Don't use without Map3D) - - - - - Displays a map with labeled clusters. - - - - - Displays a mesh. - - - - - Select a goal on a mesh. - - - diff --git a/rviz_map_plugin/src/ClusterLabelTool.cpp b/rviz_map_plugin/src/ClusterLabelTool.cpp deleted file mode 100644 index c954790..0000000 --- a/rviz_map_plugin/src/ClusterLabelTool.cpp +++ /dev/null @@ -1,737 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Robot Operating System code by the University of Osnabrück - * Copyright (c) 2015, University of Osnabrück - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above - * copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED - * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * - * - * ClusterLabelTool.cpp - * - * authors: - * Kristin Schmidt - * Jan Philipp Vogtherr - * - */ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include - -#include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelTool, rviz::Tool) - -using std::ifstream; -using std::stringstream; - -namespace rviz_map_plugin -{ -#define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" - -ClusterLabelTool::ClusterLabelTool() : m_displayInitialized(false) -{ - shortcut_key_ = 'l'; - - ros::NodeHandle n; - m_labelPublisher = n.advertise("/cluster_label", 1, true); -} - -ClusterLabelTool::~ClusterLabelTool() -{ - m_selectedFaces.clear(); - context_->getSceneManager()->destroyManualObject(m_selectionBox->getName()); - context_->getSceneManager()->destroyManualObject(m_selectionBoxMaterial->getName()); - context_->getSceneManager()->destroySceneNode(m_sceneNode); -} - -// onInitialize() is called by the superclass after scene_manager_ and -// context_ are set. It should be called only once per instantiation. -void ClusterLabelTool::onInitialize() -{ - ROS_DEBUG("ClusterLabelTool: Call Init"); - m_sceneNode = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode(); - - m_selectionBox = context_->getSceneManager()->createManualObject("ClusterLabelTool_SelectionBox"); - m_selectionBox->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY); - m_selectionBox->setUseIdentityProjection(true); - m_selectionBox->setUseIdentityView(true); - m_selectionBox->setQueryFlags(0); - m_sceneNode->attachObject(m_selectionBox); - - m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().create( - "ClusterLabelTool_SelectionBoxMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); - - m_selectionBoxMaterial->setAmbient(Ogre::ColourValue(0, 0, 255, 0.5)); - m_selectionBoxMaterial->setDiffuse(0, 0, 0, 0.5); - m_selectionBoxMaterial->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - m_selectionBoxMaterial->setDepthWriteEnabled(false); - m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); - m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); - - // try-catch block to check for OpenCL errors - try - { - // Initialize OpenCL - ROS_DEBUG("Get platforms"); - vector platforms; - cl::Platform::get(&platforms); - for (auto const& platform : platforms) - { - ROS_DEBUG("Found platform: %s", platform.getInfo().c_str()); - ROS_DEBUG("platform version: %s", platform.getInfo().c_str()); - } - ROS_DEBUG(" "); - - vector consideredDevices; - for (auto const& platform : platforms) - { - ROS_DEBUG("Get devices of %s: ", platform.getInfo().c_str()); - cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; - m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); - vector devices = m_clContext.getInfo(); - for (auto const& device : devices) - { - ROS_DEBUG("Found device: %s", device.getInfo().c_str()); - ROS_DEBUG("Device work units: %d", device.getInfo()); - ROS_DEBUG("Device work group size: %lu", device.getInfo()); - - std::string device_info = device.getInfo(); - // getVersion extracts the version number with major in the upper 16 bits and minor in the lower 16 bits - - unsigned int version = cl::detail::getVersion(std::vector(device_info.begin(), device_info.end())); - - // shift 16 to the right to get the number in the upper 16 bits - cl_uint majorVersion = version >> 16; - // use bitwise AND to extract the number in the lower 16 bits - cl_uint minorVersion = version & 0x0000FFFF; - - ROS_INFO("Found a device with OpenCL version: %u.%u", majorVersion, minorVersion); - - // find all devices that support at least OpenCL version 1.2 - if (majorVersion >= 1 && minorVersion >= 2) - ; - { - consideredDevices.push_back(device); - } - } - } - ROS_DEBUG(" "); - - cl::Platform platform; - // Preferably choose the first compatible device of type GPU - bool deviceFound = false; - for (auto const& device : consideredDevices) - { - if (device.getInfo() == CL_DEVICE_TYPE_GPU) - { - m_clDevice = device; - platform = device.getInfo(); - deviceFound = true; - break; - } - } - if (!deviceFound && consideredDevices.size() > 0) - { - // If no device of type GPU was found, choose the first compatible device - m_clDevice = consideredDevices[0]; - platform = m_clDevice.getInfo(); - deviceFound = true; - } - if (!deviceFound) - { - // Panic if no compatible device was found - ROS_ERROR("No device with compatible OpenCL version found (minimum 2.0)"); - ros::requestShutdown(); - } - - cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; - m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); - - ROS_INFO("Using device %s of platform %s", m_clDevice.getInfo().c_str(), - platform.getInfo().c_str()); - ROS_DEBUG(" "); - - // Read kernel file - ifstream in(ros::package::getPath("rviz_map_plugin") + CL_RAY_CAST_KERNEL_FILE); - std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); - - ROS_DEBUG("Got kernel: %s%s", ros::package::getPath("rviz_map_plugin").c_str(), CL_RAY_CAST_KERNEL_FILE); - - m_clProgramSources = cl::Program::Sources(1, { cast_rays_kernel.c_str(), cast_rays_kernel.length() }); - - m_clProgram = cl::Program(m_clContext, m_clProgramSources); - try - { - m_clProgram.build({ m_clDevice }); - ROS_INFO("Successfully built program."); - } - catch (cl::Error& err) - { - ROS_ERROR("Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); - - ros::shutdown(); - exit(1); - } - - // Create queue to which we will push commands for the device. - m_clQueue = cl::CommandQueue(m_clContext, m_clDevice, 0); - } - catch (cl::Error err) - { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - ros::requestShutdown(); - exit(1); - } -} - -void ClusterLabelTool::setVisual(std::shared_ptr visual) -{ - // set new visual - m_visual = visual; - m_selectedFaces = visual->getFaces(); - m_faceSelectedArray.clear(); - for (auto faceId : m_selectedFaces) - { - if (m_faceSelectedArray.size() <= faceId) - { - m_faceSelectedArray.resize(faceId + 1); - } - m_faceSelectedArray[faceId] = true; - } -} - -void ClusterLabelTool::setSphereSize(float size) -{ - m_clKernelSphere.setArg(3, size); - m_sphereSize = size; -} - -void ClusterLabelTool::activate() -{ -} - -void ClusterLabelTool::deactivate() -{ -} - -void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) -{ - m_display = display; - m_meshGeometry = m_display->getGeometry(); - m_faceSelectedArray.reserve(m_meshGeometry->faces.size()); - m_displayInitialized = true; - - m_vertexData.reserve(m_meshGeometry->faces.size() * 3 * 3); - - for (uint32_t faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) - { - for (uint32_t i = 0; i < 3; i++) - { - uint32_t vertexId = m_meshGeometry->faces[faceId].vertexIndices[i]; - Ogre::Vector3 vertexPos(m_meshGeometry->vertices[vertexId].x, m_meshGeometry->vertices[vertexId].y, - m_meshGeometry->vertices[vertexId].z); - m_vertexPositions.push_back(vertexPos); - - m_vertexData.push_back(m_meshGeometry->vertices[vertexId].x); - m_vertexData.push_back(m_meshGeometry->vertices[vertexId].y); - m_vertexData.push_back(m_meshGeometry->vertices[vertexId].z); - } - } - - // try-catch block to check for OpenCL errors - try - { - m_clVertexBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY | CL_MEM_COPY_HOST_PTR, - sizeof(float) * m_vertexData.size(), m_vertexData.data()); - - m_clResultBuffer = cl::Buffer(m_clContext, CL_MEM_WRITE_ONLY | CL_MEM_HOST_READ_ONLY, - sizeof(float) * m_meshGeometry->faces.size()); - - m_clRayBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 6); - - m_clSphereBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 4); - - m_clBoxBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 4 * 6); - - m_clStartNormalBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 3); - - m_clKernelSingleRay = cl::Kernel(m_clProgram, "cast_rays"); - m_clKernelSphere = cl::Kernel(m_clProgram, "cast_sphere"); - m_clKernelBox = cl::Kernel(m_clProgram, "cast_box"); - - m_clKernelSingleRay.setArg(0, m_clVertexBuffer); - m_clKernelSingleRay.setArg(1, m_clRayBuffer); - m_clKernelSingleRay.setArg(2, m_clResultBuffer); - - m_clKernelSphere.setArg(0, m_clVertexBuffer); - m_clKernelSphere.setArg(1, m_clSphereBuffer); - m_clKernelSphere.setArg(2, m_clResultBuffer); - m_clKernelSphere.setArg(3, m_sphereSize); - - m_clKernelBox.setArg(0, m_clVertexBuffer); - m_clKernelBox.setArg(1, m_clBoxBuffer); - m_clKernelBox.setArg(2, m_clResultBuffer); - } - catch (cl::Error err) - { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - ros::shutdown(); - exit(1); - } -} - -void ClusterLabelTool::updateSelectionBox() -{ - float left, right, top, bottom; - - left = m_selectionStart.x * 2 - 1; - right = m_selectionStop.x * 2 - 1; - top = 1 - m_selectionStart.y * 2; - bottom = 1 - m_selectionStop.y * 2; - - m_selectionBox->clear(); - m_selectionBox->begin(m_selectionBoxMaterial->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); - m_selectionBox->position(left, top, -1); - m_selectionBox->position(right, top, -1); - m_selectionBox->position(right, bottom, -1); - m_selectionBox->position(left, bottom, -1); - m_selectionBox->triangle(0, 1, 2); - m_selectionBox->triangle(0, 2, 3); - m_selectionBox->end(); -} - -void ClusterLabelTool::selectionBoxStart(rviz::ViewportMouseEvent& event) -{ - m_selectionStart.x = (float)event.x / event.viewport->getActualWidth(); - m_selectionStart.y = (float)event.y / event.viewport->getActualHeight(); - m_selectionStop = m_selectionStart; - m_selectionBox->clear(); - m_selectionBox->setVisible(true); -} - -void ClusterLabelTool::selectionBoxMove(rviz::ViewportMouseEvent& event) -{ - m_selectionStop.x = (float)event.x / event.viewport->getActualWidth(); - m_selectionStop.y = (float)event.y / event.viewport->getActualHeight(); - updateSelectionBox(); -} - -void ClusterLabelTool::selectMultipleFaces(rviz::ViewportMouseEvent& event, bool selectMode) -{ - m_selectionBox->setVisible(false); - - float left = m_selectionStart.x; - float right = m_selectionStop.x; - float top = m_selectionStart.y; - float bottom = m_selectionStop.y; - - size_t goalSection; - size_t goalIndex; - - if (left > right) - { - std::swap(left, right); - } - - if (top > bottom) - { - std::swap(top, bottom); - } - - const float BOX_SIZE_TOLERANCE = 0.0001; - if ((right - left) * (bottom - top) < BOX_SIZE_TOLERANCE) - { - selectSingleFace(event, selectMode); - return; - } - - Ogre::PlaneBoundedVolume volume = - event.viewport->getCamera()->getCameraToViewportBoxVolume(left, top, right, bottom, true); - - selectFacesInBoxParallel(volume, selectMode); -} - -void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode) -{ - m_boxData.clear(); - for (Ogre::Plane plane : volume.planes) - { - m_boxData.push_back(plane.normal.x); - m_boxData.push_back(plane.normal.y); - m_boxData.push_back(plane.normal.z); - m_boxData.push_back(plane.d); - } - - try - { - m_clQueue.enqueueWriteBuffer(m_clBoxBuffer, CL_TRUE, 0, sizeof(float) * 4 * 6, m_boxData.data()); - - m_clQueue.enqueueNDRangeKernel(m_clKernelBox, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - cl::NullRange, nullptr); - m_clQueue.finish(); - - m_resultDistances.resize(m_meshGeometry->faces.size()); - m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - m_resultDistances.data()); - } - catch (cl::Error err) - { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } - - for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) - { - if (m_resultDistances[faceId] > 0) - { - if (m_faceSelectedArray.size() <= faceId) - { - m_faceSelectedArray.resize(faceId + 1); - } - m_faceSelectedArray[faceId] = selectMode; - } - } - - std::vector tmpFaceList; - - for (uint32_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } - } - - if (m_displayInitialized && m_visual) - { - m_visual->setFacesInCluster(tmpFaceList); - // m_visual->setColor(Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f)); - } -} - -void ClusterLabelTool::selectSingleFace(rviz::ViewportMouseEvent& event, bool selectMode) -{ - Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); - selectSingleFaceParallel(ray, selectMode); -} - -void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) -{ - m_rayData = { ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z, - ray.getDirection().x, ray.getDirection().y, ray.getDirection().z }; - - std::vector> intersectedFaceList; - - try - { - m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); - - m_clQueue.enqueueNDRangeKernel(m_clKernelSingleRay, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - cl::NullRange, nullptr); - m_clQueue.finish(); - - m_resultDistances.resize(m_meshGeometry->faces.size()); - m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - m_resultDistances.data()); - } - catch (cl::Error err) - { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } - - int closestFaceId = -1; - float minDist = std::numeric_limits::max(); - - for (int i = 0; i < m_meshGeometry->faces.size(); i++) - { - if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) - { - closestFaceId = i; - minDist = m_resultDistances[i]; - } - } - - if (m_displayInitialized && m_visual && closestFaceId != -1) - { - std::vector tmpFaceList; - - if (m_faceSelectedArray.size() <= closestFaceId) - { - m_faceSelectedArray.resize(closestFaceId + 1); - } - m_faceSelectedArray[closestFaceId] = selectMode; - - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } - } - - m_visual->setFacesInCluster(tmpFaceList); - - ROS_DEBUG("selectSingleFaceParallel() found face with id %d", closestFaceId); - } -} - -void ClusterLabelTool::selectSphereFaces(rviz::ViewportMouseEvent& event, bool selectMode) -{ - Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); - selectSphereFacesParallel(ray, selectMode); -} - -void ClusterLabelTool::selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode) -{ - auto raycastResult = getClosestIntersectedFaceParallel(ray); - - if (raycastResult) - { - Ogre::Vector3 sphereCenter = ray.getPoint(raycastResult->second); - - m_sphereData = { sphereCenter.x, sphereCenter.y, sphereCenter.z, raycastResult->second }; - - try - { - m_clQueue.enqueueWriteBuffer(m_clSphereBuffer, CL_TRUE, 0, sizeof(float) * 4, m_sphereData.data()); - - m_clQueue.enqueueNDRangeKernel(m_clKernelSphere, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - cl::NullRange, nullptr); - m_clQueue.finish(); - - m_resultDistances.resize(m_meshGeometry->faces.size()); - m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - m_resultDistances.data()); - } - catch (cl::Error err) - { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } - - for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) - { - // if face is inside sphere, select it - if (m_resultDistances[faceId] > 0) - { - if (m_faceSelectedArray.size() <= faceId) - { - m_faceSelectedArray.resize(faceId + 1); - } - m_faceSelectedArray[faceId] = selectMode; - } - } - - if (m_displayInitialized && m_visual) - { - std::vector tmpFaceList; - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - tmpFaceList.push_back(faceId); - } - } - - m_visual->setFacesInCluster(tmpFaceList); - } - } -} - -boost::optional> ClusterLabelTool::getClosestIntersectedFaceParallel(Ogre::Ray& ray) -{ - m_rayData = { ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z, - ray.getDirection().x, ray.getDirection().y, ray.getDirection().z }; - - try - { - m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); - - m_clQueue.enqueueNDRangeKernel(m_clKernelSingleRay, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), - cl::NullRange, nullptr); - m_clQueue.finish(); - - m_resultDistances.resize(m_meshGeometry->faces.size()); - m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), - m_resultDistances.data()); - } - catch (cl::Error err) - { - ROS_ERROR_STREAM(err.what() << ": " << CLUtil::getErrorString(err.err())); - ROS_WARN_STREAM("(" << CLUtil::getErrorDescription(err.err()) << ")"); - } - - uint32_t closestFaceId; - bool faceFound = false; - float minDist = std::numeric_limits::max(); - - for (uint32_t i = 0; i < m_meshGeometry->faces.size(); i++) - { - if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) - { - closestFaceId = i; - faceFound = true; - minDist = m_resultDistances[i]; - } - } - - if (faceFound) - { - return std::make_pair(closestFaceId, minDist); - } - else - { - return {}; - } -} - -void ClusterLabelTool::publishLabel(std::string label) -{ - ROS_DEBUG_STREAM("Label Tool: Publish label '" << label << "'"); - - vector faces; - for (uint32_t i = 0; i < m_faceSelectedArray.size(); i++) - { - if (m_faceSelectedArray[i]) - faces.push_back(i); - } - - m_display->addLabel(label, faces); -} - -// Handling mouse event and mark the clicked faces -int ClusterLabelTool::processMouseEvent(rviz::ViewportMouseEvent& event) -{ - if (event.leftDown() && event.control()) - { - m_singleSelect = true; - selectSphereFaces(event, true); - } - else if (event.leftUp() && m_singleSelect) - { - m_singleSelect = false; - selectSphereFaces(event, true); - } - else if (event.rightDown() && event.control()) - { - m_singleDeselect = true; - selectSphereFaces(event, false); - } - else if (event.rightUp() && m_singleDeselect) - { - m_singleDeselect = false; - selectSphereFaces(event, false); - } - else if (event.leftDown()) - { - m_multipleSelect = true; - selectionBoxStart(event); - } - else if (event.leftUp() && m_multipleSelect) - { - m_multipleSelect = false; - selectMultipleFaces(event, true); - } - else if (event.rightDown()) - { - m_multipleSelect = true; - selectionBoxStart(event); - } - else if (event.rightUp() && m_multipleSelect) - { - m_multipleSelect = false; - selectMultipleFaces(event, false); - } - else if (m_multipleSelect) - { - selectionBoxMove(event); - } - else if (m_singleSelect) - { - selectSphereFaces(event, true); - } - else if (m_singleDeselect) - { - selectSphereFaces(event, false); - } - - return Render; -} - -std::vector ClusterLabelTool::getSelectedFaces() -{ - std::vector faceList; - - for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) - { - if (m_faceSelectedArray[faceId]) - { - faceList.push_back(faceId); - } - } - return faceList; -} - -void ClusterLabelTool::resetFaces() -{ - m_faceSelectedArray.clear(); - if (m_visual) - { - m_visual->setFacesInCluster(std::vector()); - } -} - -void ClusterLabelTool::resetVisual() -{ - m_visual.reset(); -} - -} // End namespace rviz_map_plugin diff --git a/rviz_map_plugin/src/MapDisplay.cpp b/rviz_map_plugin/src/MapDisplay.cpp deleted file mode 100644 index d6e40d6..0000000 --- a/rviz_map_plugin/src/MapDisplay.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Robot Operating System code by the University of Osnabrück - * Copyright (c) 2015, University of Osnabrück - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above - * copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED - * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * - * - * MapDisplay.cpp - * - * - * authors: - * - * Kristin Schmidt - * Jan Philipp Vogtherr - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace rviz_map_plugin -{ -MapDisplay::MapDisplay() -{ - m_mapFilePath = new rviz::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, - SLOT(updateMap())); -} - -MapDisplay::~MapDisplay() -{ -} - -// ===================================================================================================================== -// Public Q_SLOTS - -std::shared_ptr MapDisplay::getGeometry() -{ - if (!m_geometry) - { - ROS_ERROR("Map Display: Geometry requested, but none available!"); - } - return m_geometry; -} - -// ===================================================================================================================== -// Callbacks - -rviz::Display* MapDisplay::createDisplay(const QString& class_id) -{ - rviz::DisplayFactory* factory = context_->getDisplayFactory(); - QString error; - rviz::Display* disp = factory->make(class_id, &error); - if (!disp) - { - return new rviz::FailedDisplay(class_id, error); - } - return disp; -} - -void MapDisplay::onInitialize() -{ - Display* display = createDisplay("rviz_map_plugin/ClusterLabel"); - - m_clusterLabelDisplay = static_cast(display); - m_clusterLabelDisplay->setName("ClusterLabel"); - m_clusterLabelDisplay->setModel(model_); - m_clusterLabelDisplay->setParent(this); - addChild(m_clusterLabelDisplay); - m_clusterLabelDisplay->initialize(context_); - - Display* meshDisplay = createDisplay("rviz_map_plugin/Mesh"); - - m_meshDisplay = static_cast(meshDisplay); - addChild(m_meshDisplay); - m_meshDisplay->setName("Mesh"); - m_meshDisplay->setModel(model_); - m_meshDisplay->setParent(this); - m_meshDisplay->initialize(context_); - m_meshDisplay->ignoreIncomingMessages(); - - // Make signal/slot connections - connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); - - updateMap(); -} - -void MapDisplay::onEnable() -{ - m_clusterLabelDisplay->onEnable(); - m_meshDisplay->onEnable(); -} - -void MapDisplay::onDisable() -{ - m_clusterLabelDisplay->onDisable(); - m_meshDisplay->onDisable(); -} - -// ===================================================================================================================== -// Callbacks triggered from UI events (mostly) - -void MapDisplay::updateMap() -{ - ROS_INFO("Map Display: Update"); - - // Load geometry and clusters - bool successful = loadData(); - if (!successful) - return; - - // Update sub-plugins - m_meshDisplay->setGeometry(m_geometry); - m_meshDisplay->setVertexColors(m_colors); - m_meshDisplay->setVertexNormals(m_normals); - m_meshDisplay->clearVertexCosts(); - for (const auto& vertexCosts : m_costs) - { - std::vector costs = vertexCosts.second; - m_meshDisplay->addVertexCosts(vertexCosts.first, costs); - } - m_meshDisplay->setMaterials(m_materials, m_texCoords); - // m_meshDisplay->setTexCoords(m_texCoords); - for (uint32_t i = 0; i < m_textures.size(); i++) - { - m_meshDisplay->addTexture(m_textures[i], i); - } - m_clusterLabelDisplay->setData(m_geometry, m_clusterList); - - // All good - setStatus(rviz::StatusProperty::Ok, "Map", ""); -} - -// ===================================================================================================================== -// Data loading - -bool MapDisplay::loadData() -{ - // Read map file path - std::string mapFile = m_mapFilePath->getFilename(); - if (mapFile.empty()) - { - ROS_WARN_STREAM("Map Display: No map file path specified!"); - setStatus(rviz::StatusProperty::Warn, "Map", "No map file path specified!"); - return false; - } - if (!boost::filesystem::exists(mapFile)) - { - ROS_WARN_STREAM("Map Display: Specified map file does not exist!"); - setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file does not exist!"); - return false; - } - if (boost::filesystem::extension(mapFile).compare(".h5") != 0) - { - ROS_WARN_STREAM("Map Display: Specified map file is not a .h5 file!"); - setStatus(rviz::StatusProperty::Warn, "Map", "Specified map file is not a .h5 file!"); - return false; - } - ROS_INFO_STREAM("Map Display: Loading data for map '" << mapFile << "'"); - - try - { - // Open file IO - hdf5_map_io::HDF5MapIO map_io(mapFile); - - ROS_INFO("Map Display: Load geometry"); - - // Read geometry - m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); - - ROS_INFO("Map Display: Load textures"); - - // Read textures - vector textures = map_io.getTextures(); - m_textures.resize(textures.size()); - for (size_t i = 0; i < textures.size(); i++) - { - // Find out the texture index because textures are not stored in ascending order - int textureIndex = std::stoi(textures[i].name); - - // Copy metadata - m_textures[textureIndex].width = textures[i].width; - m_textures[textureIndex].height = textures[i].height; - m_textures[textureIndex].channels = textures[i].channels; - m_textures[textureIndex].data = textures[i].data; - m_textures[textureIndex].pixelFormat = "rgb8"; - } - - ROS_INFO("Map Display: Load materials"); - - // Read materials - vector materials = map_io.getMaterials(); - vector faceToMaterialIndexArray = map_io.getMaterialFaceIndices(); - m_materials.resize(materials.size()); - for (size_t i = 0; i < materials.size(); i++) - { - // Copy material color - m_materials[i].color.r = materials[i].r / 255.0f; - m_materials[i].color.g = materials[i].g / 255.0f; - m_materials[i].color.b = materials[i].b / 255.0f; - m_materials[i].color.a = 1.0f; - - // Look for texture index - if (materials[i].textureIndex == -1) - { - // texture index -1: no texture - m_materials[i].textureIndex = boost::none; - } - else - { - m_materials[i].textureIndex = materials[i].textureIndex; - } - - m_materials[i].faceIndices.clear(); - } - - // Copy face indices - for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) - { - m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); - } - - ROS_INFO("Map Display: Load vertex colors"); - - // Read vertex colors - vector colors = map_io.getVertexColors(); - m_colors.clear(); - m_colors.reserve(colors.size() / 3); - for (size_t i = 0; i < colors.size(); i += 3) - { - // convert from 0-255 (uint8) to 0.0-1.0 (float) - m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); - } - - ROS_INFO("Map Display: Load vertex normals"); - - // Read vertex normals - vector normals = map_io.getVertexNormals(); - m_normals.clear(); - m_normals.reserve(normals.size() / 3); - for (size_t i = 0; i < normals.size(); i += 3) - { - m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); - } - - ROS_INFO("Map Display: Load texture coordinates"); - - // Read tex cords - vector texCoords = map_io.getVertexTextureCoords(); - m_texCoords.clear(); - m_texCoords.reserve(texCoords.size() / 3); - for (size_t i = 0; i < texCoords.size(); i += 3) - { - m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); - } - - ROS_INFO("Map Display: Load clusters"); - - // Read labels - m_clusterList.clear(); - // m_clusterList.push_back(Cluster("__NEW__", vector())); - for (auto labelGroup : map_io.getLabelGroups()) - { - for (auto labelObj : map_io.getAllLabelsOfGroup(labelGroup)) - { - auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); - - std::stringstream ss; - ss << labelGroup << "_" << labelObj; - std::string label = ss.str(); - - m_clusterList.push_back(Cluster(label, faceIds)); - } - } - - m_costs.clear(); - for (std::string costlayer : map_io.getCostLayers()) - { - try - { - m_costs[costlayer] = map_io.getVertexCosts(costlayer); - } - catch (const hf::DataSpaceException& e) - { - ROS_WARN_STREAM("Could not load channel " << costlayer << " as a costlayer!"); - } - } - } - catch (...) - { - ROS_ERROR_STREAM("An unexpected error occurred while using Pluto Map IO"); - setStatus(rviz::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); - return false; - } - - setStatus(rviz::StatusProperty::Ok, "IO", ""); - - ROS_INFO("Map Display: Successfully loaded map."); - - return true; -} - -// ===================================================================================================================== -// Label - -void MapDisplay::saveLabel(Cluster cluster) -{ - std::string label = cluster.name; - std::vector faces = cluster.faces; - - ROS_INFO_STREAM("Map Display: add label '" << label << "'"); - - try - { - // Split label into class and instance (tree_1 => class "tree" & instance "1") - std::vector results; - boost::split(results, label, [](char c) { return c == '_'; }); - if (results.size() != 2) - { - ROS_ERROR_STREAM("Map Display: Illegal label name '" << label << "'"); - setStatus(rviz::StatusProperty::Error, "Label", "Illegal label name!"); - return; - } - - // Open IO - hdf5_map_io::HDF5MapIO map_io(m_mapFilePath->getFilename()); - - // Add label with faces list - map_io.addOrUpdateLabel(results[0], results[1], faces); - - // Add to cluster list - m_clusterList.push_back(Cluster(label, faces)); - - setStatus(rviz::StatusProperty::Ok, "Label", "Successfully saved label"); - ROS_INFO_STREAM("Map Display: Successfully added label to map."); - - // update the map to show the new label - updateMap(); - } - catch (...) - { - setStatus(rviz::StatusProperty::Error, "Label", "Error while saving label"); - } -} - -} // End namespace rviz_map_plugin - -#include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MapDisplay, rviz::Display) diff --git a/rviz_map_plugin/src/MeshPoseTool.cpp b/rviz_map_plugin/src/MeshPoseTool.cpp deleted file mode 100644 index 54938a0..0000000 --- a/rviz_map_plugin/src/MeshPoseTool.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Robot Operating System code by the University of Osnabrück - * Copyright (c) 2015, University of Osnabrück - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above - * copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED - * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * - * - * MeshPoseTool.cpp - * - * authors: Sebastian Pütz - * - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "MeshPoseTool.hpp" - -namespace rviz_map_plugin -{ -MeshPoseTool::MeshPoseTool() : rviz::Tool(), arrow_(NULL) -{ -} - -MeshPoseTool::~MeshPoseTool() -{ - delete arrow_; -} - -void MeshPoseTool::onInitialize() -{ - arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); - arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); - arrow_->getSceneNode()->setVisible(false); -} - -void MeshPoseTool::activate() -{ - setStatus("Click and on a mesh_msgs::TriangleMesh to set the position and drag the mouse for the orientation."); - state_ = Position; -} - -void MeshPoseTool::deactivate() -{ - arrow_->getSceneNode()->setVisible(false); -} - -int MeshPoseTool::processMouseEvent(rviz::ViewportMouseEvent& event) -{ - int flags = 0; - - if (event.leftDown()) - { - ROS_ASSERT(state_ == Position); - - Ogre::Vector3 pos, ori; - if (selectTriangle(event, pos, ori)) - { - pos_ = pos; - ori_ = ori; - arrow_->setPosition(pos_); - state_ = Orientation; - flags |= Render; - } - } - else if (event.type == QEvent::MouseMove && event.left()) - { - if (state_ == Orientation) - { - Ogre::Vector3 cur_pos; - Ogre::Plane plane(ori_, pos_); - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) - { - // right hand coordinate system - // x to the right, y to the top, and -z into the scene - // arrow foreward negative z - Ogre::Vector3 z_axis = -(cur_pos - pos_); - Ogre::Vector3 y_axis = ori_; - Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis); - - x_axis.normalise(); - y_axis.normalise(); - z_axis.normalise(); - - arrow_->getSceneNode()->setVisible(true); - arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); - - flags |= Render; - } - } - } - else if (event.leftUp()) - { - if (state_ == Orientation) - { - Ogre::Vector3 cur_pos; - Ogre::Plane plane(ori_, pos_); - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, cur_pos)) - { - // arrow foreward negative z - Ogre::Vector3 z_axis = -(cur_pos - pos_); - Ogre::Vector3 y_axis = ori_; - Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis); - - x_axis.normalise(); - y_axis.normalise(); - z_axis.normalise(); - - onPoseSet(pos_, Ogre::Quaternion(x_axis, y_axis, z_axis)); - - flags |= (Finished | Render); - } - } - } - - return flags; -} - -bool MeshPoseTool::selectTriangle(rviz::ViewportMouseEvent& event, Ogre::Vector3& position, - Ogre::Vector3& triangle_normal) -{ - Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( - (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); - - Ogre::RaySceneQuery* query = - context_->getSceneManager()->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); - query->setSortByDistance(true); - - Ogre::RaySceneQueryResult& result = query->execute(); - - for (size_t i = 0; i < result.size(); i++) - { - if (result[i].movable->getName().find("TriangleMesh") != std::string::npos) - { - Ogre::ManualObject* mesh = static_cast(result[i].movable); - size_t goal_section = -1; - size_t goal_index = -1; - Ogre::Real dist = -1; - if (getPositionAndOrientation(mesh, ray, position, triangle_normal)) - { - return true; - } - } - } - return false; -} - -bool MeshPoseTool::getPositionAndOrientation(const Ogre::ManualObject* mesh, const Ogre::Ray& ray, - Ogre::Vector3& position, Ogre::Vector3& orientation) -{ - Ogre::Real dist = -1.0f; - Ogre::Vector3 a, b, c; - - size_t vertex_count = 0; - Ogre::Vector3* vertices; - size_t index_count = 0; - unsigned long* indices; - size_t num_sections = mesh->getNumSections(); - - for (size_t i = 0; i < num_sections; i++) - { - getRawManualObjectData(mesh, i, vertex_count, vertices, index_count, indices); - if (index_count != 0) - { - for (size_t j = 0; j < index_count; j += 3) - { - std::pair goal = Ogre::Math::intersects(ray, vertices[indices[j]], vertices[indices[j + 1]], - vertices[indices[j + 2]], true, true); - - if (goal.first) - { - if ((dist < 0.0f) || (goal.second < dist)) - { - dist = goal.second; - a = vertices[indices[j]]; - b = vertices[indices[j + 1]]; - c = vertices[indices[j + 2]]; - } - } - } - } - } - - delete[] vertices; - delete[] indices; - if (dist != -1) - { - position = ray.getPoint(dist); - Ogre::Vector3 ab = b - a; - Ogre::Vector3 ac = c - a; - orientation = ac.crossProduct(ab).normalisedCopy(); - return true; - } - else - { - return false; - } -} - -void MeshPoseTool::getRawManualObjectData(const Ogre::ManualObject* mesh, const size_t sectionNumber, - size_t& vertexCount, Ogre::Vector3*& vertices, size_t& indexCount, - unsigned long*& indices) -{ - Ogre::VertexData* vertexData; - const Ogre::VertexElement* vertexElement; - Ogre::HardwareVertexBufferSharedPtr vertexBuffer; - unsigned char* vertexChar; - float* vertexFloat; - - vertexData = mesh->getSection(sectionNumber)->getRenderOperation()->vertexData; - vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION); - vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource()); - vertexChar = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); - - vertexCount = vertexData->vertexCount; - vertices = new Ogre::Vector3[vertexCount]; - - for (size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize()) - { - vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat); - vertices[i] = - (mesh->getParentNode()->_getDerivedOrientation() * - (Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]) * mesh->getParentNode()->_getDerivedScale())) + - mesh->getParentNode()->_getDerivedPosition(); - } - - vertexBuffer->unlock(); - - Ogre::IndexData* indexData; - Ogre::HardwareIndexBufferSharedPtr indexBuffer; - indexData = mesh->getSection(sectionNumber)->getRenderOperation()->indexData; - indexCount = indexData->indexCount; - indices = new unsigned long[indexCount]; - indexBuffer = indexData->indexBuffer; - unsigned int* pLong = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); - unsigned short* pShort = reinterpret_cast(pLong); - - for (size_t i = 0; i < indexCount; i++) - { - unsigned long index; - if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT) - { - index = static_cast(pLong[i]); - } - - else - { - index = static_cast(pShort[i]); - } - - indices[i] = index; - } - indexBuffer->unlock(); -} - -} // namespace rviz_map_plugin diff --git a/rviz_map_plugin/CHANGELOG.rst b/rviz_mesh_tools_plugins/CHANGELOG.rst similarity index 94% rename from rviz_map_plugin/CHANGELOG.rst rename to rviz_mesh_tools_plugins/CHANGELOG.rst index f132a26..89b1da1 100644 --- a/rviz_map_plugin/CHANGELOG.rst +++ b/rviz_mesh_tools_plugins/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rviz_map_plugin +Changelog for package rviz_mesh_tools_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.1.0 (2021-10-27) @@ -21,7 +21,7 @@ Changelog for package rviz_map_plugin * MeshDisplay now woring with MeshMap * added properties to MeshDisplay * added Map3D file dialog property -* added textured mesh display for mesh msgs visualization to rviz_map_plugin +* added textured mesh display for mesh msgs visualization to rviz_mesh_tools_plugins * combined mesh-plugin and map-plugin texture mesh visuals * fixed mpi error * resolved catkin lint problems diff --git a/rviz_mesh_tools_plugins/CMakeLists.txt b/rviz_mesh_tools_plugins/CMakeLists.txt new file mode 100644 index 0000000..b5a5195 --- /dev/null +++ b/rviz_mesh_tools_plugins/CMakeLists.txt @@ -0,0 +1,175 @@ +cmake_minimum_required(VERSION 3.8) +project(rviz_mesh_tools_plugins) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_definitions(-D_BUILD_DIR_PATH="${CMAKE_CURRENT_BINARY_DIR}") +add_definitions(-D_SRC_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}") + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(rviz_assimp_vendor REQUIRED) +find_package(std_msgs REQUIRED) +find_package(hdf5_map_io REQUIRED) +find_package(mesh_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(message_filters REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(Boost REQUIRED COMPONENTS system filesystem) +find_package(HDF5 REQUIRED COMPONENTS C CXX HL) +# find_package(assimp REQUIRED) + +# This needs to be optional for non CL devices +# find_package(OpenCL 2 REQUIRED) + +# include_directories(${ASSIMP_INCLUDE_DIRS}) + +## This setting causes Qt's "MOC" generation to happen automatically. +# set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOMOC ON) +find_package(Qt5 COMPONENTS Core Widgets) +# add_definitions(-DQT_NO_KEYWORDS) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${HDF5_INCLUDE_DIRS} + ${OpenCL_INCLUDE_DIRS} +) + +set(SOURCE_FILES + src/ClusterLabelDisplay.cpp + src/ClusterLabelPanel.cpp + src/ClusterLabelTool.cpp + src/ClusterLabelVisual.cpp + src/MapDisplay.cpp + src/MeshDisplay.cpp + src/MeshVisual.cpp + src/RvizFileProperty.cpp + src/MeshPoseTool.cpp + src/MeshGoalTool.cpp + src/InteractionHelper.cpp +) + + +set(MOC_HEADER_FILES + include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp + include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp + include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp + include/rviz_mesh_tools_plugins/MapDisplay.hpp + include/rviz_mesh_tools_plugins/MeshDisplay.hpp + include/rviz_mesh_tools_plugins/MeshVisual.hpp + include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp + include/rviz_mesh_tools_plugins/CLUtil.hpp + include/rviz_mesh_tools_plugins/RvizFileProperty.hpp + include/rviz_mesh_tools_plugins/MeshPoseTool.hpp + include/rviz_mesh_tools_plugins/MeshGoalTool.hpp + include/rviz_mesh_tools_plugins/InteractionHelper.hpp +) + +# foreach(header "${RVIZ_MESH_TOOLS_PLUGINS_MOC_FILES}") +# qt5_wrap_cpp(RVIZ_MESH_TOOLS_PLUGINS_MOC "${header}") +# endforeach() + + + +add_library(${PROJECT_NAME} SHARED + ${SOURCE_FILES} + ${MOC_HEADER_FILES} + ${HDF5_LIBRARIES} + ${HDF5_HL_LIBRARIES} + ${OpenCL_LIBRARIES} +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay + Boost::system + Boost::filesystem +) + +target_link_libraries(${PROJECT_NAME} PRIVATE + ${HDF5_LIBRARIES} + ${HDF5_HL_LIBRARIES} + ${OpenCL_LIBRARIES} +) + +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_MESH_TOOLS_PLUGINS_BUILDING_LIBRARY")# + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies(${PROJECT_NAME} + PUBLIC + rclcpp + rmw + rviz_common + rviz_rendering + rviz_assimp_vendor + std_msgs + hdf5_map_io + mesh_msgs + resource_retriever + tf2_ros + message_filters + std_msgs +) + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") + +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_export_dependencies( + rclcpp + rmw + rviz_common + rviz_rendering + rviz_assimp_vendor + std_msgs + hdf5_map_io + mesh_msgs + resource_retriever + tf2_ros + message_filters +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION share/${PROJECT_NAME}) + +ament_package( + CONFIG_EXTRAS "rviz_mesh_tools_plugins-extras.cmake" +) \ No newline at end of file diff --git a/rviz_map_plugin/README.md b/rviz_mesh_tools_plugins/README.md similarity index 100% rename from rviz_map_plugin/README.md rename to rviz_mesh_tools_plugins/README.md diff --git a/rviz_map_plugin/icons/classes/ClusterLabel.png b/rviz_mesh_tools_plugins/icons/classes/ClusterLabel.png similarity index 100% rename from rviz_map_plugin/icons/classes/ClusterLabel.png rename to rviz_mesh_tools_plugins/icons/classes/ClusterLabel.png diff --git a/rviz_map_plugin/icons/classes/Map3D.png b/rviz_mesh_tools_plugins/icons/classes/Map3D.png similarity index 100% rename from rviz_map_plugin/icons/classes/Map3D.png rename to rviz_mesh_tools_plugins/icons/classes/Map3D.png diff --git a/rviz_map_plugin/icons/classes/Mesh.png b/rviz_mesh_tools_plugins/icons/classes/Mesh.png similarity index 100% rename from rviz_map_plugin/icons/classes/Mesh.png rename to rviz_mesh_tools_plugins/icons/classes/Mesh.png diff --git a/rviz_map_plugin/icons/classes/MeshGoal.png b/rviz_mesh_tools_plugins/icons/classes/MeshGoal.png similarity index 100% rename from rviz_map_plugin/icons/classes/MeshGoal.png rename to rviz_mesh_tools_plugins/icons/classes/MeshGoal.png diff --git a/rviz_map_plugin/include/CLUtil.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp similarity index 99% rename from rviz_map_plugin/include/CLUtil.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp index fd366e7..100d380 100644 --- a/rviz_map_plugin/include/CLUtil.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/CLUtil.hpp @@ -50,7 +50,7 @@ #include -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class CLUtil @@ -471,6 +471,6 @@ class CLUtil }; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp similarity index 77% rename from rviz_map_plugin/include/ClusterLabelDisplay.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp index 8d3ac4c..59d86ba 100644 --- a/rviz_map_plugin/include/ClusterLabelDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelDisplay.hpp @@ -49,7 +49,7 @@ #ifndef CLUSTER_LABEL_DISPLAY_HPP #define CLUSTER_LABEL_DISPLAY_HPP -#include +#include #include #include @@ -67,45 +67,49 @@ #include #include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +// #include +// #include +#include +#include +#include +#include + +#include +#include -#ifndef Q_MOC_RUN -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include -#endif -namespace rviz + +#ifndef Q_MOC_RUN +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#endif // Q_MOC_RUN + +namespace rviz_common +{ +namespace properties { // Forward declaration class BoolProperty; @@ -114,10 +118,10 @@ class FloatProperty; class IntProperty; class EnumProperty; class StringProperty; +} // namespace properties +} // namespace rviz_common -} // End namespace rviz - -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { using std::map; using std::shared_ptr; @@ -133,7 +137,7 @@ class ClusterLabelTool; * @class ClusterLabelDisplay * @brief Display class for the map plugin */ -class ClusterLabelDisplay : public rviz::Display +class ClusterLabelDisplay : public rviz_common::Display { Q_OBJECT @@ -266,30 +270,32 @@ private Q_SLOTS: ClusterLabelTool* m_tool; /// Property for the current active visual - rviz::EnumProperty* m_activeVisualProperty; + rviz_common::properties::EnumProperty* m_activeVisualProperty; /// Property to set transparency - rviz::FloatProperty* m_alphaProperty; + rviz_common::properties::FloatProperty* m_alphaProperty; /// Property for selecting colors (menu) - rviz::Property* m_colorsProperty; + rviz_common::properties::Property* m_colorsProperty; /// Properties for selecting colors (menu-items) - std::vector m_colorProperties; + std::vector m_colorProperties; /// Property to set the brushsize of the sphere brush of the label tool from this package - rviz::FloatProperty* m_sphereSizeProperty; + rviz_common::properties::FloatProperty* m_sphereSizeProperty; /// Property to hide or show a phantom visual - rviz::BoolProperty* m_phantomVisualProperty; + rviz_common::properties::BoolProperty* m_phantomVisualProperty; /// Index for the visuals int m_labelToolVisualIndex = 0; /// A variable that will be set to true, once the initial data has arrived bool has_data = false; + + }; -} // end namespace rviz_map_plugin +} // namespace rviz_mesh_tools_plugins -#endif +#endif // CLUSTER_LABEL_DISPLAY_HPP diff --git a/rviz_map_plugin/include/ClusterLabelPanel.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp similarity index 86% rename from rviz_map_plugin/include/ClusterLabelPanel.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp index a10ca7e..a8197b7 100644 --- a/rviz_map_plugin/include/ClusterLabelPanel.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelPanel.hpp @@ -44,18 +44,18 @@ * * Kristin Schmidt * Jan Philipp Vogtherr + * Alexander Mock */ #ifndef CLUSTER_LABEL_PANEL_HPP #define CLUSTER_LABEL_PANEL_HPP -#include -#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include // Forward declarations class QLineEdit; @@ -66,13 +66,13 @@ namespace rviz class Tool; } -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class ClusterLabelPanel * @brief Panel for interacting with the label tool */ -class ClusterLabelPanel : public rviz::Panel +class ClusterLabelPanel : public rviz_common::Panel { Q_OBJECT @@ -92,13 +92,13 @@ class ClusterLabelPanel : public rviz::Panel * @brief Load a configuration * @input config The configuration */ - virtual void load(const rviz::Config& config); + virtual void load(const rviz_common::Config& config); /** * @brief Save a configuration * @input config The configuration */ - virtual void save(rviz::Config config) const; + virtual void save(rviz_common::Config config) const; public Q_SLOTS: @@ -141,9 +141,9 @@ public Q_SLOTS: ClusterLabelTool* m_tool; /// Node handle - ros::NodeHandle m_nodeHandle; + // ros::NodeHandle m_nodeHandle; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp similarity index 66% rename from rviz_map_plugin/include/ClusterLabelTool.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp index 8deb6af..9741b00 100644 --- a/rviz_map_plugin/include/ClusterLabelTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelTool.hpp @@ -48,14 +48,16 @@ #ifndef CLUSTER_LABEL_TOOL_HPP #define CLUSTER_LABEL_TOOL_HPP -// enable exceptions for OpenCL -#define CL_HPP_TARGET_OPENCL_VERSION 120 -#define CL_HPP_MINIMUM_OPENCL_VERSION 110 -#define CL_HPP_ENABLE_EXCEPTIONS -#include -#include +#include + +// TODO: Make CL optional +// enable exceptions for OpenCL +// #define CL_HPP_TARGET_OPENCL_VERSION 120 +// #define CL_HPP_MINIMUM_OPENCL_VERSION 110 +// #define CL_HPP_ENABLE_EXCEPTIONS +// #include #include #include @@ -70,49 +72,57 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include +#include +#include +#include + +#include #ifndef Q_MOC_RUN -#include +#include + -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -#endif +#include -namespace rviz +#endif // Q_MOC_RUN + +#include + +namespace rviz_common +{ +namespace properties { class RosTopicProperty; class ColorProperty; -} // namespace rviz +} // namespace properties +} // namespace rviz_common // OGRE stuff namespace Ogre { // Forward declaration -class Vector3; +// class Vector3; } // namespace Ogre -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { // Forward declarations class ClusterLabelDisplay; @@ -122,7 +132,7 @@ class ClusterLabelVisual; * @class ClusterLabelTool * @brief Tool for selecting faces */ -class ClusterLabelTool : public rviz::Tool +class ClusterLabelTool : public rviz_common::Tool { Q_OBJECT public: @@ -134,7 +144,7 @@ class ClusterLabelTool : public rviz::Tool /** * @brief Destructor */ - ~ClusterLabelTool(); + virtual ~ClusterLabelTool(); /** * @brief RViz callback on initialize @@ -156,7 +166,7 @@ class ClusterLabelTool : public rviz::Tool * @param event The mouse event * @return Exit code */ - virtual int processMouseEvent(rviz::ViewportMouseEvent& event); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event); /** * @brief Connects this tool with a given display @@ -210,12 +220,21 @@ public Q_SLOTS: float m_sphereSize = 1.0f; // Selection Box - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; Ogre::SceneNode* m_sceneNode; Ogre::ManualObject* m_selectionBox; Ogre::MaterialPtr m_selectionBoxMaterial; - Ogre::Vector2 m_selectionStart; - Ogre::Vector2 m_selectionStop; + + // rviz_common::ViewportMouseEvent m_evt_start; + // rviz_common::ViewportMouseEvent m_evt_stop; + + int m_bb_x1; + int m_bb_y1; + int m_bb_x2; + int m_bb_y2; + + rviz_common::RenderPanel* m_evt_panel; + bool m_multipleSelect = false; bool m_singleSelect = false; bool m_singleDeselect = false; @@ -223,17 +242,17 @@ public Q_SLOTS: std::vector m_vertexPositions; void updateSelectionBox(); - void selectionBoxStart(rviz::ViewportMouseEvent& event); - void selectionBoxMove(rviz::ViewportMouseEvent& event); - void selectMultipleFaces(rviz::ViewportMouseEvent& event, bool selectMode); + void selectionBoxStart(rviz_common::ViewportMouseEvent& event); + void selectionBoxMove(rviz_common::ViewportMouseEvent& event); + void selectMultipleFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); void selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode); - void selectSingleFace(rviz::ViewportMouseEvent& event, bool selectMode); + void selectSingleFace(rviz_common::ViewportMouseEvent& event, bool selectMode); void selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode); - void selectSphereFaces(rviz::ViewportMouseEvent& event, bool selectMode); + void selectSphereFaces(rviz_common::ViewportMouseEvent& event, bool selectMode); void selectSphereFacesParallel(Ogre::Ray& ray, bool selectMode); boost::optional> getClosestIntersectedFaceParallel(Ogre::Ray& ray); - ros::Publisher m_labelPublisher; + rclcpp::Publisher::SharedPtr m_labelPublisher; std::vector m_vertexData; std::array m_rayData; @@ -243,22 +262,22 @@ public Q_SLOTS: std::vector m_resultDistances; // OpenCL - cl::Device m_clDevice; - cl::Context m_clContext; - cl::Program::Sources m_clProgramSources; - cl::Program m_clProgram; - cl::CommandQueue m_clQueue; - cl::Buffer m_clVertexBuffer; - cl::Buffer m_clResultBuffer; - cl::Buffer m_clRayBuffer; - cl::Buffer m_clSphereBuffer; - cl::Buffer m_clBoxBuffer; - cl::Buffer m_clStartNormalBuffer; - cl::Kernel m_clKernelSingleRay; - cl::Kernel m_clKernelSphere; - cl::Kernel m_clKernelBox; - cl::Kernel m_clKernelDirAndDist; + // cl::Device m_clDevice; + // cl::Context m_clContext; + // cl::Program::Sources m_clProgramSources; + // cl::Program m_clProgram; + // cl::CommandQueue m_clQueue; + // cl::Buffer m_clVertexBuffer; + // cl::Buffer m_clResultBuffer; + // cl::Buffer m_clRayBuffer; + // cl::Buffer m_clSphereBuffer; + // cl::Buffer m_clBoxBuffer; + // cl::Buffer m_clStartNormalBuffer; + // cl::Kernel m_clKernelSingleRay; + // cl::Kernel m_clKernelSphere; + // cl::Kernel m_clKernelBox; + // cl::Kernel m_clKernelDirAndDist; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_map_plugin/include/ClusterLabelVisual.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp similarity index 86% rename from rviz_map_plugin/include/ClusterLabelVisual.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp index c21f13e..a280ef7 100644 --- a/rviz_map_plugin/include/ClusterLabelVisual.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ClusterLabelVisual.hpp @@ -48,18 +48,18 @@ #ifndef CLUSTER_LABEL_VISUAL_HPP #define CLUSTER_LABEL_VISUAL_HPP -#include +#include -#include +// #include -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -71,7 +71,7 @@ class SceneNode; class Mesh; } // End namespace Ogre -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { /** * @class ClusterLabelVisual @@ -86,7 +86,9 @@ class ClusterLabelVisual * @param context The context that contains the display information. * @param labelId The label id (that has to be unique) */ - ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId); + // ClusterLabelVisual( + // rviz_common::DisplayContext* context, + // std::string labelId); /** * @brief Constructor @@ -95,12 +97,15 @@ class ClusterLabelVisual * @param labelId The label id (that has to be unique) * @param geometry A shared pointer to the geometry to which the labels belong */ - ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId, std::shared_ptr geometry); + ClusterLabelVisual( + rviz_common::DisplayContext* context, + std::string labelId, + std::shared_ptr geometry); /** * @brief Destructor */ - ~ClusterLabelVisual(); + virtual ~ClusterLabelVisual(); /** * @brief Disabling the copy constructor @@ -158,7 +163,7 @@ class ClusterLabelVisual private: void initMaterial(); - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; Ogre::SceneNode* m_sceneNode; std::string m_labelId; @@ -172,6 +177,6 @@ class ClusterLabelVisual std::vector m_faces; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp new file mode 100644 index 0000000..55203b0 --- /dev/null +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/InteractionHelper.hpp @@ -0,0 +1,41 @@ +#ifndef RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP +#define RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP + +#include +#include + + +namespace rviz_common +{ + class DisplayContext; +} // namespace rviz_common + +namespace rviz_mesh_tools_plugins +{ + +struct Intersection { + double range; + Ogre::Vector3 point; + Ogre::Vector3 normal; + uint32_t face_id; + uint32_t geom_id; // currently unused + uint32_t inst_id; // currently unused +}; + +Ogre::Ray getMouseEventRay( + const rviz_common::ViewportMouseEvent& event); + +bool selectFace( + const Ogre::ManualObject* mesh, + const Ogre::Ray& ray, + Intersection& intersection); + +bool selectFace( + const rviz_common::DisplayContext* ctx, + const Ogre::Ray& ray, + Intersection& intersection); + +} // namespace rviz_mesh_tools_plugins + + +#endif // RVIZ_MESH_TOOLS_PLUGINS_MOUSE_EVENT_HELPER_HPP \ No newline at end of file diff --git a/rviz_map_plugin/include/MapDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp similarity index 68% rename from rviz_map_plugin/include/MapDisplay.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp index db9cc96..24b741b 100644 --- a/rviz_map_plugin/include/MapDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MapDisplay.hpp @@ -49,8 +49,8 @@ #ifndef MAP_DISPLAY_HPP #define MAP_DISPLAY_HPP -#include -#include "RvizFileProperty.hpp" +#include +#include #include #include @@ -67,52 +67,54 @@ #include #include #include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include + +#include +#include +// #include +// #include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include #include #ifndef Q_MOC_RUN -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include +#endif // Q_MOCK_RUN + +#include +#include namespace rviz { +namespace properties +{ // Forward declaration class BoolProperty; class ColorProperty; @@ -120,10 +122,10 @@ class FloatProperty; class IntProperty; class EnumProperty; class StringProperty; +} // namespace properties +} // namespace rviz -} // End namespace rviz - -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { using std::shared_ptr; using std::string; @@ -134,7 +136,7 @@ using std::vector; * @class MapDisplay * @brief Master display for the Mesh- and Cluster- subdisplays. THis implementation uses HDF5 as it's data source */ -class MapDisplay : public rviz::Display +class MapDisplay : public rviz_common::Display { Q_OBJECT @@ -149,6 +151,8 @@ class MapDisplay : public rviz::Display */ ~MapDisplay(); + virtual void load(const rviz_common::Config& config); + public Q_SLOTS: /** @@ -171,6 +175,10 @@ private Q_SLOTS: void updateMap(); private: + void enableClusterLabelDisplay(); + void disableClusterLabelDisplay(); + void enableMeshDisplay(); + void disableMeshDisplay(); /** * @brief RViz callback on initialize */ @@ -192,6 +200,8 @@ private Q_SLOTS: */ bool loadData(); + + // TODO: make more efficient - currently everything is stored in the MapDisplay, the MeshDisplay and the MeshVisual /// Geometry shared_ptr m_geometry; @@ -211,21 +221,22 @@ private Q_SLOTS: std::map> m_costs; /// Path to map file - rviz::FileProperty* m_mapFilePath; + rviz_common::properties::FileProperty* m_mapFilePath; + std::string m_map_file_loaded; /// Subdisplay: ClusterLabel (for showing the clusters) - rviz_map_plugin::ClusterLabelDisplay* m_clusterLabelDisplay; + rviz_mesh_tools_plugins::ClusterLabelDisplay* m_clusterLabelDisplay; /// Subdisplay: MeshDisplay (for showing the mesh) - rviz_map_plugin::MeshDisplay* m_meshDisplay; + rviz_mesh_tools_plugins::MeshDisplay* m_meshDisplay; /** * @brief Create a RViz display from it's unique class_id * @param class_id The class ID * @return Pointer to RViz display */ - rviz::Display* createDisplay(const QString& class_id); + rviz_common::Display* createDisplay(const QString& class_id); }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_map_plugin/include/MeshDisplay.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp similarity index 67% rename from rviz_map_plugin/include/MeshDisplay.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp index 9a85972..7a0a164 100644 --- a/rviz_map_plugin/include/MeshDisplay.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp @@ -54,8 +54,8 @@ #ifndef MESH_DISPLAY_HPP #define MESH_DISPLAY_HPP -#include -#include +#include +#include #include #include @@ -68,37 +68,47 @@ #include #include #include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include + +#include +#include +#include +#include + #ifndef Q_MOC_RUN #include #include #include -#include +#include -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#endif +#include +#include +#include +#include +#include + +#endif // Q_MOC_RUN -namespace rviz +#include "rclcpp/rclcpp.hpp" + +namespace rviz_common +{ +namespace properties { // Forward declaration class BoolProperty; @@ -108,10 +118,10 @@ class IntProperty; class RosTopicProperty; class EnumProperty; class StringProperty; +} // end namespace properties +} // end namespace rviz_common -} // End namespace rviz - -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { using std::shared_ptr; using std::string; @@ -125,12 +135,12 @@ class MeshVisual; * @class MeshDisplay * @brief Display for showing the mesh in different modes */ -class MeshDisplay : public rviz::Display +class MeshDisplay : public rviz_common::Display { Q_OBJECT public: - /** + /**#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" * @brief Constructor */ MeshDisplay(); @@ -172,7 +182,7 @@ class MeshDisplay : public rviz::Display * @brief Set the geometry * @param geometry The geometry */ - void setGeometry(shared_ptr geometry); + void setGeometry(std::shared_ptr geometry); /** * @brief Set the vertex colors @@ -296,25 +306,25 @@ private Q_SLOTS: * @brief Sets data for trianglemesh_visual and updates the mesh. * @param meshMsg Message containing geometry information */ - void processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg); + void processMessage(const mesh_msgs::msg::MeshGeometryStamped& meshMsg); /** * @brief Handler for incoming geometry messages. Validate data and update mesh * @param meshMsg The geometry */ - void incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg); + void incomingGeometry(const mesh_msgs::msg::MeshGeometryStamped::ConstSharedPtr& meshMsg); /** * @brief Handler for incoming vertex color messages. Validate data and update mesh * @param colorsStamped The vertex colors */ - void incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr& colorsStamped); + void incomingVertexColors(const mesh_msgs::msg::MeshVertexColorsStamped::ConstSharedPtr& colorsStamped); /** * @brief Handler for incoming vertex cost messages. Validate data and update mesh * @param costsStamped The vertex costs */ - void incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr& costsStamped); + void incomingVertexCosts(const mesh_msgs::msg::MeshVertexCostsStamped::ConstSharedPtr& costsStamped); /** * @brief Requests vertex colors from the specified service @@ -349,47 +359,59 @@ private Q_SLOTS: /// if set to true, ignore incoming messages and do not use services to request materials bool m_ignoreMsgs; + + // TODO: use this instead: + // ros_integration::RosNodeAbstractionIface::WeakPtr m_node; + // from + // #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" + // std::shared_ptr m_node; + + /// Client to request the vertex colors - ros::ServiceClient m_vertexColorClient; + rclcpp::Client::SharedPtr m_vertexColorClient; /// Client to request the materials - ros::ServiceClient m_materialsClient; + rclcpp::Client::SharedPtr m_materialsClient; /// Client to request the textures - ros::ServiceClient m_textureClient; + rclcpp::Client::SharedPtr m_textureClient; /// Client to request the UUID - ros::ServiceClient m_uuidClient; + rclcpp::Client::SharedPtr m_uuidClient; /// Client to request the geometry - ros::ServiceClient m_geometryClient; + rclcpp::Client::SharedPtr m_geometryClient; /// Subscriber for meshMsg - message_filters::Subscriber m_meshSubscriber; + message_filters::Subscriber m_meshSubscriber; /// Subscriber for vertex colors - message_filters::Subscriber m_vertexColorsSubscriber; + message_filters::Subscriber m_vertexColorsSubscriber; /// Subscriber for vertex costs - message_filters::Subscriber m_vertexCostsSubscriber; + message_filters::Subscriber m_vertexCostsSubscriber; /// Messagefilter for meshMsg - tf2_ros::MessageFilter* m_tfMeshFilter; + + + + + tf2_ros::RVizMessageFilterPtr m_tfMeshFilter; /// Messagefilter for vertex colors - tf2_ros::MessageFilter* m_tfVertexColorsFilter; + tf2_ros::RVizMessageFilterPtr m_tfVertexColorsFilter; /// Messagefilter for vertex costs - tf2_ros::MessageFilter* m_tfVertexCostsFilter; + tf2_ros::RVizMessageFilterPtr m_tfVertexCostsFilter; /// Synchronizer for meshMsgs - message_filters::Cache* m_meshSynchronizer; + message_filters::Cache* m_meshSynchronizer; /// Synchronizer for vertex colors - message_filters::Cache* m_colorsSynchronizer; + message_filters::Cache* m_colorsSynchronizer; /// Synchronizer for vertex costs - message_filters::Cache* m_costsSynchronizer; + message_filters::Cache* m_costsSynchronizer; /// Counter for the received messages uint32_t m_messagesReceived; @@ -401,78 +423,78 @@ private Q_SLOTS: std::queue> m_visuals; /// Property to handle topic for meshMsg - rviz::RosTopicProperty* m_meshTopic; + rviz_common::properties::RosTopicProperty* m_meshTopic; /// Property to handle buffer size - rviz::IntProperty* m_bufferSize; + rviz_common::properties::IntProperty* m_bufferSize; /// Property to select the display type - rviz::EnumProperty* m_displayType; + rviz_common::properties::EnumProperty* m_displayType; /// Property to set faces color - rviz::ColorProperty* m_facesColor; + rviz_common::properties::ColorProperty* m_facesColor; /// Property to set faces transparency - rviz::FloatProperty* m_facesAlpha; + rviz_common::properties::FloatProperty* m_facesAlpha; /// Property to handle topic for vertex colors - rviz::RosTopicProperty* m_vertexColorsTopic; + rviz_common::properties::RosTopicProperty* m_vertexColorsTopic; /// Property to handle service name for vertexColors - rviz::StringProperty* m_vertexColorServiceName; + rviz_common::properties::StringProperty* m_vertexColorServiceName; /// Property to only show textured faces when texturizing is enabled - rviz::BoolProperty* m_showTexturedFacesOnly; + rviz_common::properties::BoolProperty* m_showTexturedFacesOnly; /// Property to handle service name for materials - rviz::StringProperty* m_materialServiceName; + rviz_common::properties::StringProperty* m_materialServiceName; /// Property to handle service name for textures - rviz::StringProperty* m_textureServiceName; + rviz_common::properties::StringProperty* m_textureServiceName; /// Property for selecting the color type for cost display - rviz::EnumProperty* m_costColorType; + rviz_common::properties::EnumProperty* m_costColorType; /// Property to handle topic for vertex cost maps - rviz::RosTopicProperty* m_vertexCostsTopic; + rviz_common::properties::RosTopicProperty* m_vertexCostsTopic; /// Property to select different types of vertex cost maps to be shown - rviz::EnumProperty* m_selectVertexCostMap; + rviz_common::properties::EnumProperty* m_selectVertexCostMap; /// Property for using custom limits for cost display - rviz::BoolProperty* m_costUseCustomLimits; + rviz_common::properties::BoolProperty* m_costUseCustomLimits; /// Property for setting the lower limit of cost display - rviz::FloatProperty* m_costLowerLimit; + rviz_common::properties::FloatProperty* m_costLowerLimit; /// Property for setting the upper limit of cost display - rviz::FloatProperty* m_costUpperLimit; + rviz_common::properties::FloatProperty* m_costUpperLimit; /// Property to select the normals - rviz::BoolProperty* m_showNormals; + rviz_common::properties::BoolProperty* m_showNormals; /// Property to set the color of the normals - rviz::ColorProperty* m_normalsColor; + rviz_common::properties::ColorProperty* m_normalsColor; /// Property to set the transparency of the normals - rviz::FloatProperty* m_normalsAlpha; + rviz_common::properties::FloatProperty* m_normalsAlpha; /// Property to set the size of the normals - rviz::FloatProperty* m_scalingFactor; + rviz_common::properties::FloatProperty* m_scalingFactor; /// Property to select the wireframe - rviz::BoolProperty* m_showWireframe; + rviz_common::properties::BoolProperty* m_showWireframe; /// Property to set wireframe color - rviz::ColorProperty* m_wireframeColor; + rviz_common::properties::ColorProperty* m_wireframeColor; /// Property to set wireframe transparency - rviz::FloatProperty* m_wireframeAlpha; + rviz_common::properties::FloatProperty* m_wireframeAlpha; /// Cache for received vertex cost messages std::map> m_costCache; }; -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_map_plugin/include/MeshGoalTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp similarity index 81% rename from rviz_map_plugin/include/MeshGoalTool.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp index cff9695..004a395 100644 --- a/rviz_map_plugin/include/MeshGoalTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshGoalTool.hpp @@ -45,17 +45,19 @@ #ifndef MESH_GOAL_TOOL_HPP #define MESH_GOAL_TOOL_HPP -#include "MeshPoseTool.hpp" -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifndef Q_MOC_RUN #include -#endif +#endif // Q_MOC_RUN -namespace rviz_map_plugin +#include + +namespace rviz_mesh_tools_plugins { /** * @class MeshGoalTool @@ -91,15 +93,15 @@ private Q_SLOTS: virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); /// Property for the topic - rviz::StringProperty* topic_property_; + rviz_common::properties::StringProperty* topic_property_; /// Switch bottom / top for selection - rviz::BoolProperty* switch_bottom_top_; + rviz_common::properties::BoolProperty* switch_bottom_top_; /// Publisher - ros::Publisher pose_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; /// Node handle - ros::NodeHandle nh_; + // ros::NodeHandle nh_; }; -} /* namespace rviz_map_plugin */ +} // namespace rviz_mesh_tools_plugins -#endif +#endif // MESH_GOAL_TOOL_HPP diff --git a/rviz_map_plugin/include/MeshPoseTool.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp similarity index 70% rename from rviz_map_plugin/include/MeshPoseTool.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp index 0c0b62c..751229a 100644 --- a/rviz_map_plugin/include/MeshPoseTool.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshPoseTool.hpp @@ -45,19 +45,24 @@ #ifndef MESH_POSE_TOOL_HPP #define MESH_POSE_TOOL_HPP -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include -#include +#include +// #include -namespace rviz_map_plugin +// Forward declare types +namespace rviz_rendering { -class MeshPoseTool : public rviz::Tool + class Arrow; +} + +namespace rviz_mesh_tools_plugins +{ +class MeshPoseTool : public rviz_common::Tool { public: MeshPoseTool(); @@ -68,30 +73,25 @@ class MeshPoseTool : public rviz::Tool virtual void activate(); virtual void deactivate(); - virtual int processMouseEvent(rviz::ViewportMouseEvent& event); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event); protected: virtual void onPoseSet(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) = 0; - void getRawManualObjectData(const Ogre::ManualObject* mesh, const size_t sectionNumber, size_t& vertexCount, - Ogre::Vector3*& vertices, size_t& indexCount, unsigned long*& indices); - - bool getPositionAndOrientation(const Ogre::ManualObject* mesh, const Ogre::Ray& ray, Ogre::Vector3& position, - Ogre::Vector3& orientation); - - bool selectTriangle(rviz::ViewportMouseEvent& event, Ogre::Vector3& position, Ogre::Vector3& orientation); - - rviz::Arrow* arrow_; + rviz_rendering::Arrow* arrow_; enum State { Position, Orientation }; State state_; - Ogre::Vector3 pos_; - Ogre::Vector3 ori_; + Ogre::Vector3 pos_start_; + Ogre::Vector3 normal_start_; + Ogre::Vector3 pos_last_; + // smooth normal in pixel coordinates > 2 + unsigned smooth_normal_width_ = 9; }; -} /* namespace rviz_map_plugin */ +} /* namespace rviz_mesh_tools_plugins */ #endif diff --git a/rviz_map_plugin/include/MeshVisual.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp similarity index 92% rename from rviz_map_plugin/include/MeshVisual.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp index c191e1e..c1537e7 100644 --- a/rviz_map_plugin/include/MeshVisual.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshVisual.hpp @@ -53,43 +53,42 @@ #ifndef MESH_VISUAL_HPP #define MESH_VISUAL_HPP -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include #include namespace Ogre { // Forward declaration -class Vector3; class Quaternion; class SceneNode; class Entity; } // End namespace Ogre -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { /** * @brief Class to display mesh data in the main panel of rviz. @@ -105,7 +104,7 @@ class MeshVisual * @param meshID The mesh id * @param randomID random number that will be used as part of the meshes UID */ - MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID); + MeshVisual(rviz_common::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID); /** * @brief Destructor. @@ -321,7 +320,7 @@ class MeshVisual Ogre::SceneNode* m_sceneNode; /// The context that contains the display information. - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; /// First ID of the created mesh size_t m_prefix; @@ -380,6 +379,6 @@ class MeshVisual /// raw normals std::vector m_geometryNormals; }; -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_tools_plugins #endif diff --git a/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp new file mode 100644 index 0000000..97d02e6 --- /dev/null +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RVizMessageFilter.hpp @@ -0,0 +1,18 @@ +#ifndef RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP +#define RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP + +#include +#include + +namespace tf2_ros +{ + +template +using RVizMessageFilter = tf2_ros::MessageFilter; + +template +using RVizMessageFilterPtr = std::shared_ptr >; + +} // namespace tf2_ros + +#endif // RVIZ_MESH_TOOLS_PLUGINS_RVIZ_MESSAGE_FILTER_HPP \ No newline at end of file diff --git a/rviz_map_plugin/include/RvizFileProperty.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp similarity index 81% rename from rviz_map_plugin/include/RvizFileProperty.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp index 5540b9c..89ae3e0 100644 --- a/rviz_map_plugin/include/RvizFileProperty.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/RvizFileProperty.hpp @@ -43,23 +43,32 @@ * authors: * * Malte kleine Piening + * Alexander Mock */ #ifndef RVIZ_FILE_PROPERTY_HPP #define RVIZ_FILE_PROPERTY_HPP #include -#include +#include -namespace rviz +namespace rviz_common { -class FileProperty : public Property + +namespace properties +{ + +class FileProperty : public FilePickerProperty { Q_OBJECT public: - FileProperty(const QString& name = QString(), const QString& default_value = QString(), - const QString& description = QString(), Property* parent = nullptr, const char* changed_slot = nullptr, - QObject* receiver = nullptr); + FileProperty( + const QString& name = QString(), + const QString& default_value = QString(), + const QString& description = QString(), + Property* parent = nullptr, + const char* changed_slot = nullptr, + QObject* receiver = nullptr); std::string getFilename() { @@ -79,6 +88,7 @@ public Q_SLOTS: } }; -} // end namespace rviz +} // namespace properties +} // namespace rviz -#endif +#endif // RVIZ_FILE_PROPERTY_HPP diff --git a/rviz_map_plugin/include/Types.hpp b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp similarity index 97% rename from rviz_map_plugin/include/Types.hpp rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp index fde90a9..3e0149b 100644 --- a/rviz_map_plugin/include/Types.hpp +++ b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/Types.hpp @@ -44,6 +44,7 @@ * * Kristin Schmidt * Jan Philipp Vogtherr + * Alexander Mock */ #pragma once @@ -53,13 +54,15 @@ #include #include -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { using boost::optional; using std::array; using std::string; using std::vector; + + /// Struct for normals struct Normal { @@ -72,6 +75,8 @@ struct Normal } }; + + /// Struct for texture coordinates struct TexCoords { @@ -174,4 +179,4 @@ struct Material vector faceIndices; }; -} // namespace rviz_map_plugin +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_map_plugin/include/kernels/cast_rays.cl b/rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/kernels/cast_rays.cl similarity index 100% rename from rviz_map_plugin/include/kernels/cast_rays.cl rename to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/kernels/cast_rays.cl diff --git a/rviz_mesh_tools_plugins/package.xml b/rviz_mesh_tools_plugins/package.xml new file mode 100644 index 0000000..cf70504 --- /dev/null +++ b/rviz_mesh_tools_plugins/package.xml @@ -0,0 +1,45 @@ + + + + rviz_mesh_tools_plugins + + RViz display types and tools for the mesh_msgs package. + + 1.1.0 + Sebastian Pütz + Sebastian Pütz + Kristin Schmidt + Jan Philipp Vogtherr + Malte kleine Piening + catkin + + BSD-3 + https://github.com/uos/mesh_tools/tree/master/rviz_mesh_tools_plugins + + ament_cmake + + qtbase5-dev + rviz_ogre_vendor + + rclcpp + rviz_common + rviz_rendering + rviz_assimp_vendor + std_msgs + mesh_msgs + hdf5_map_io + tf2_ros + pluginlib + message_filters + ocl-icd-opencl-dev + opencl-headers + + libqt5-core + libqt5-gui + libqt5-widgets + rviz_ogre_vendor + + + ament_cmake + + diff --git a/rviz_mesh_tools_plugins/plugins_description.xml b/rviz_mesh_tools_plugins/plugins_description.xml new file mode 100644 index 0000000..e87094d --- /dev/null +++ b/rviz_mesh_tools_plugins/plugins_description.xml @@ -0,0 +1,44 @@ + + + + A panel widget allowing creation of cluster labels. + + + + + A tool allowing selection of clusters. + + + + + Displays labeled clusters of a map. (Don't use without Map3D) + + + + + Displays a map with labeled clusters. + + + + + Displays a mesh. + + + + + Select a goal on a mesh. + + + diff --git a/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake b/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake new file mode 100644 index 0000000..d07309a --- /dev/null +++ b/rviz_mesh_tools_plugins/rviz_mesh_tools_plugins-extras.cmake @@ -0,0 +1,47 @@ +# Software License Agreement (BSD License) +# +# Robot Operating System code by the University of Osnabrück +# Copyright (c) 2023, University of Osnabrück +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above +# copyright notice, this list of conditions and the following +# disclaimer. +# +# 2. Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +# TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR +# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +# OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +# OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# +# +# rviz_mesh_tools_plugins-extras.cmake +# +# +# authors: +# +# Alexander Mock +# +find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) +find_package(Boost REQUIRED QUITE COMPONENTS system filesystem) \ No newline at end of file diff --git a/rviz_map_plugin/src/ClusterLabelDisplay.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp similarity index 70% rename from rviz_map_plugin/src/ClusterLabelDisplay.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp index 356dc71..e838685 100644 --- a/rviz_map_plugin/src/ClusterLabelDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelDisplay.cpp @@ -46,18 +46,20 @@ * Jan Philipp Vogtherr */ -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace rviz_map_plugin +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace rviz_mesh_tools_plugins { Ogre::ColourValue getRainbowColor(float value) { @@ -92,25 +94,25 @@ Ogre::ColourValue getRainbowColor(float value) ClusterLabelDisplay::ClusterLabelDisplay() { m_activeVisualProperty = - new rviz::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", + new rviz_common::properties::EnumProperty("Active label", "__NEW__", "Current active label. Can be edited with Cluster Label Tool", this, SLOT(changeVisual()), this); - m_alphaProperty = new rviz::FloatProperty("Transparency", 1.0f, + m_alphaProperty = new rviz_common::properties::FloatProperty("Transparency", 1.0f, "Transparency of the Labeled Cluster Visualization. 0.0 is fully " "transparent, 1.0 fully opaque", this, SLOT(updateColors()), this); m_alphaProperty->setMin(0.0f); m_alphaProperty->setMax(1.0f); - m_colorsProperty = new rviz::Property("Colors", "", "colors", this, SLOT(updateColors()), this); + m_colorsProperty = new rviz_common::properties::Property("Colors", "", "colors", this, SLOT(updateColors()), this); m_colorsProperty->setReadOnly(true); m_sphereSizeProperty = - new rviz::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); - m_phantomVisualProperty = new rviz::BoolProperty("Show Phantom", false, + new rviz_common::properties::FloatProperty("Brush Size", 1.0f, "Brush Size", this, SLOT(updateSphereSize()), this); + m_phantomVisualProperty = new rviz_common::properties::BoolProperty("Show Phantom", false, "Show a transparent silhouette of the whole mesh to help with " "labeling", this, SLOT(updatePhantomVisual()), this); - setStatus(rviz::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); + setStatus(rviz_common::properties::StatusProperty::Error, "Display", "Cant be used without Map3D plugin"); } ClusterLabelDisplay::~ClusterLabelDisplay() @@ -124,7 +126,7 @@ std::shared_ptr ClusterLabelDisplay::getGeometry() { if (!m_geometry) { - ROS_ERROR("Label Display: Geometry requested, but none available!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Geometry requested, but none available!"); } return m_geometry; } @@ -133,7 +135,7 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector { if (has_data) { - ROS_WARN("Label Display: already has data, but setData() was called again!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: already has data, but setData() was called again!"); } // Copy data @@ -142,14 +144,16 @@ void ClusterLabelDisplay::setData(shared_ptr geometry, vector m_clusterList.insert(m_clusterList.begin(), Cluster("__NEW__", vector())); // Set flag - ROS_INFO("Label Display: received data"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: received data"); has_data = true; // Draw visuals - if (isEnabled()) + if(isEnabled()) + { updateMap(); + } - setStatus(rviz::StatusProperty::Ok, "Display", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); } // ===================================================================================================================== @@ -180,11 +184,11 @@ void ClusterLabelDisplay::changeVisual() { if (m_activeVisualProperty->getStdString().empty()) { - ROS_ERROR("Label Display: Should change visual but no visual selected!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Should change visual but no visual selected!"); return; } - ROS_INFO_STREAM("Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Changed active visual to '" << m_activeVisualProperty->getStdString() << "'"); m_activeVisualId = m_activeVisualProperty->getOptionInt(); @@ -194,16 +198,21 @@ void ClusterLabelDisplay::changeVisual() void ClusterLabelDisplay::updateMap() { - ROS_INFO("Label Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Update"); - if (!has_data) + if(!has_data) { - ROS_WARN("Label Display: No data available! Can't show map"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: No data available! Can't show map"); return; } - // Reset the visual of the label tool so that it can be deleted - m_tool->resetVisual(); + if(m_tool) + { + // Reset the visual of the label tool so that it can be deleted + m_tool->resetVisual(); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Tool not initialized!"); + } // Now create the visuals for the loaded clusters createVisualsFromClusterList(); @@ -224,7 +233,7 @@ void ClusterLabelDisplay::updateMap() m_tool->setDisplay(this); // All good - setStatus(rviz::StatusProperty::Ok, "Map", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); } void ClusterLabelDisplay::updateColors() @@ -267,7 +276,7 @@ void ClusterLabelDisplay::fillPropertyOptions() // Add color options Ogre::ColourValue rainbowColor = getRainbowColor((((float)i + 1) / m_clusterList.size())); - m_colorProperties.emplace_back(new rviz::ColorProperty( + m_colorProperties.emplace_back(new rviz_common::properties::ColorProperty( QString::fromStdString(m_clusterList[i].name), QColor(rainbowColor.r * 255, rainbowColor.g * 255, rainbowColor.b * 255), QString::fromStdString(m_clusterList[i].name), m_colorsProperty, SLOT(updateColors()), this)); @@ -293,7 +302,7 @@ void ClusterLabelDisplay::createVisualsFromClusterList() ss << "ClusterLabelVisual_" << i; auto visual = std::make_shared(context_, ss.str(), m_geometry); - ROS_DEBUG_STREAM("Label Display: Create visual for label '" << m_clusterList[i].name << "'"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Display: Create visual for label '" << m_clusterList[i].name << "'"); visual->setFacesInCluster(m_clusterList[i].faces); visual->setColor(getRainbowColor((++colorIndex / m_clusterList.size())), m_alphaProperty->getFloat()); m_visuals.push_back(visual); @@ -318,12 +327,12 @@ void ClusterLabelDisplay::createPhantomVisual() void ClusterLabelDisplay::initializeLabelTool() { // Check if the cluster label tool is already opened - rviz::ToolManager* toolManager = context_->getToolManager(); + rviz_common::ToolManager* toolManager = context_->getToolManager(); QStringList toolClasses = toolManager->getToolClasses(); bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) { - if (toolClasses.at(i).contains("ClusterLabel")) + if (toolClasses[i].contains("ClusterLabel")) { m_tool = static_cast(toolManager->getTool(i)); foundTool = true; @@ -333,23 +342,29 @@ void ClusterLabelDisplay::initializeLabelTool() if (!foundTool) { - m_tool = static_cast(context_->getToolManager()->addTool("rviz_map_plugin/ClusterLabel")); + auto tool_tmp = context_->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel"); + if(m_tool = dynamic_cast(tool_tmp); m_tool != nullptr) + { + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Created ClusterLabelTool"); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not create ClusterLabelTool"); + } } } void ClusterLabelDisplay::notifyLabelTool() { - m_tool->setVisual(m_visuals[m_activeVisualId]); + m_tool->setVisual(m_visuals[m_activeVisualId]); } void ClusterLabelDisplay::addLabel(std::string label, std::vector faces) { - ROS_INFO_STREAM("Cluster Label Display: add label '" << label << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cluster Label Display: add label '" << label << "'"); Q_EMIT signalAddLabel(Cluster(label, faces)); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_tools_plugins -#include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelDisplay, rviz::Display) +#include +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/ClusterLabelPanel.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp similarity index 80% rename from rviz_map_plugin/src/ClusterLabelPanel.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp index 4ff4728..79c8cec 100644 --- a/rviz_map_plugin/src/ClusterLabelPanel.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelPanel.cpp @@ -46,7 +46,7 @@ * Jan Philipp Vogtherr */ -#include +#include #include #include @@ -56,11 +56,12 @@ #include #include -#include +#include -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { -ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz::Panel(parent) +ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) +:rviz_common::Panel(parent) { QHBoxLayout* clusterNameLayout = new QHBoxLayout(); clusterNameLayout->addWidget(new QLabel("Cluster Name:")); @@ -87,7 +88,8 @@ ClusterLabelPanel::ClusterLabelPanel(QWidget* parent) : rviz::Panel(parent) void ClusterLabelPanel::onInitialize() { // Check if the cluster label tool is already opened - rviz::ToolManager* toolManager = vis_manager_->getToolManager(); + + rviz_common::ToolManager* toolManager = this->getDisplayContext()->getToolManager(); QStringList toolClasses = toolManager->getToolClasses(); bool foundTool = false; for (int i = 0; i < toolClasses.size(); i++) @@ -102,7 +104,7 @@ void ClusterLabelPanel::onInitialize() if (!foundTool) { - m_tool = static_cast(vis_manager_->getToolManager()->addTool("rviz_map_plugin/ClusterLabel")); + m_tool = static_cast(this->getDisplayContext()->getToolManager()->addTool("rviz_mesh_tools_plugins/ClusterLabel")); } } @@ -122,35 +124,34 @@ void ClusterLabelPanel::updateClusterName() void ClusterLabelPanel::publish() { - ROS_INFO("Label Panel: Publish"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Panel: Publish"); m_tool->publishLabel(m_clusterName.toStdString()); } void ClusterLabelPanel::resetFaces() { - ROS_INFO("Label panel: Reset"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label panel: Reset"); m_tool->resetFaces(); } -void ClusterLabelPanel::save(rviz::Config config) const +void ClusterLabelPanel::save(rviz_common::Config config) const { - rviz::Panel::save(config); + rviz_common::Panel::save(config); config.mapSetValue("ClusterName", m_clusterName); } -void ClusterLabelPanel::load(const rviz::Config& config) +void ClusterLabelPanel::load(const rviz_common::Config& config) { - rviz::Panel::load(config); + rviz_common::Panel::load(config); QString clusterName; if (config.mapGetString("ClusterName", &clusterName)) - ; { m_clusterNameEditor->setText(clusterName); updateClusterName(); } } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_tools_plugins -#include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::ClusterLabelPanel, rviz::Panel) +#include +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelPanel, rviz_common::Panel) diff --git a/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp new file mode 100644 index 0000000..7a7fe98 --- /dev/null +++ b/rviz_mesh_tools_plugins/src/ClusterLabelTool.cpp @@ -0,0 +1,905 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * ClusterLabelTool.cpp + * + * authors: + * Kristin Schmidt + * Jan Philipp Vogtherr + * + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include + +#include + +#include +#include +#include +#include + +#include + + + +using std::ifstream; +using std::stringstream; + +namespace rviz_mesh_tools_plugins +{ +// #define CL_RAY_CAST_KERNEL_FILE "/include/kernels/cast_rays.cl" + + + +ClusterLabelTool::ClusterLabelTool() +:rviz_common::Tool() +,m_displayInitialized(false) +// ,m_evt_start(nullptr, (QMouseEvent*)nullptr, 0, 0) +// ,m_evt_stop(nullptr, (QMouseEvent*)nullptr, 0, 0) +{ + shortcut_key_ = 'l'; +} + +ClusterLabelTool::~ClusterLabelTool() +{ + m_selectedFaces.clear(); + context_->getSceneManager()->destroyManualObject(m_selectionBox->getName()); + context_->getSceneManager()->destroyManualObject(m_selectionBoxMaterial->getName()); + context_->getSceneManager()->destroySceneNode(m_sceneNode); +} + +// onInitialize() is called by the superclass after scene_manager_ and +// context_ are set. It should be called only once per instantiation. +void ClusterLabelTool::onInitialize() +{ + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelTool: Call Init"); + + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + m_labelPublisher = node->create_publisher( + "/cluster_label", 1); + + m_sceneNode = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode(); + + m_selectionBox = context_->getSceneManager()->createManualObject("ClusterLabelTool_SelectionBox"); + m_selectionBox->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY); + m_selectionBox->setUseIdentityProjection(true); + m_selectionBox->setUseIdentityView(true); + m_selectionBox->setQueryFlags(0); + m_sceneNode->attachObject(m_selectionBox); + + m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().getByName( + "ClusterLabelTool_SelectionBoxMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + + if(!m_selectionBoxMaterial) + { + m_selectionBoxMaterial = Ogre::MaterialManager::getSingleton().create( + "ClusterLabelTool_SelectionBoxMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); + } + + m_selectionBoxMaterial->setAmbient(Ogre::ColourValue(0, 0, 255, 0.5)); + m_selectionBoxMaterial->setDiffuse(0, 0, 0, 0.5); + m_selectionBoxMaterial->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + m_selectionBoxMaterial->setDepthWriteEnabled(false); + m_selectionBoxMaterial->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID); + m_selectionBoxMaterial->setCullingMode(Ogre::CULL_NONE); + + + // // try-catch block to check for OpenCL errors + // try + // { + // // Initialize OpenCL + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Get platforms"); + // vector platforms; + // cl::Platform::get(&platforms); + // for (auto const& platform : platforms) + // { + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found platform: %s", platform.getInfo().c_str()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "platform version: %s", platform.getInfo().c_str()); + // } + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); + + // vector consideredDevices; + // for (auto const& platform : platforms) + // { + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Get devices of %s: ", platform.getInfo().c_str()); + // cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; + // m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); + // vector devices = m_clContext.getInfo(); + // for (auto const& device : devices) + // { + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found device: %s", device.getInfo().c_str()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Device work units: %d", device.getInfo()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Device work group size: %lu", device.getInfo()); + + // std::string device_info = device.getInfo(); + // // getVersion extracts the version number with major in the upper 16 bits and minor in the lower 16 bits + + // unsigned int version = cl::detail::getVersion(std::vector(device_info.begin(), device_info.end())); + + // // shift 16 to the right to get the number in the upper 16 bits + // cl_uint majorVersion = version >> 16; + // // use bitwise AND to extract the number in the lower 16 bits + // cl_uint minorVersion = version & 0x0000FFFF; + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found a device with OpenCL version: %u.%u", majorVersion, minorVersion); + + // // find all devices that support at least OpenCL version 1.2 + // if (majorVersion >= 1 && minorVersion >= 2) + // ; + // { + // consideredDevices.push_back(device); + // } + // } + // } + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); + + // cl::Platform platform; + // // Preferably choose the first compatible device of type GPU + // bool deviceFound = false; + // for (auto const& device : consideredDevices) + // { + // if (device.getInfo() == CL_DEVICE_TYPE_GPU) + // { + // m_clDevice = device; + // platform = device.getInfo(); + // deviceFound = true; + // break; + // } + // } + // if (!deviceFound && consideredDevices.size() > 0) + // { + // // If no device of type GPU was found, choose the first compatible device + // m_clDevice = consideredDevices[0]; + // platform = m_clDevice.getInfo(); + // deviceFound = true; + // } + // if (!deviceFound) + // { + // // Panic if no compatible device was found + // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "No device with compatible OpenCL version found (minimum 2.0)"); + // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + // rclcpp::shutdown(); + // } + + // cl_context_properties properties[] = { CL_CONTEXT_PLATFORM, (cl_context_properties)(platform)(), 0 }; + // m_clContext = cl::Context(CL_DEVICE_TYPE_ALL, properties); + + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Using device %s of platform %s", m_clDevice.getInfo().c_str(), + // platform.getInfo().c_str()); + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), " "); + + // // Read kernel file + // // may throw ament_index_cpp::PackageNotFoundError exception + // std::string package_share_directory = ament_index_cpp::get_package_share_directory("my_package_name"); + + // ifstream in(package_share_directory + CL_RAY_CAST_KERNEL_FILE); + // std::string cast_rays_kernel(static_cast(stringstream() << in.rdbuf()).str()); + + + // RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Got kernel: %s%s", package_share_directory.c_str(), CL_RAY_CAST_KERNEL_FILE); + + // m_clProgramSources = cl::Program::Sources(1, { cast_rays_kernel.c_str(), cast_rays_kernel.length() }); + + // m_clProgram = cl::Program(m_clContext, m_clProgramSources); + // try + // { + // m_clProgram.build({ m_clDevice }); + // RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successfully built program."); + // } + // catch (cl::Error& err) + // { + + // RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Error building: %s", m_clProgram.getBuildInfo(m_clDevice).c_str()); + // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + // rclcpp::shutdown(); + // // ros::shutdown(); + // exit(1); + // } + + // // Create queue to which we will push commands for the device. + // m_clQueue = cl::CommandQueue(m_clContext, m_clDevice, 0); + // } + // catch (cl::Error err) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + // rclcpp::shutdown(); + // // ros::requestShutdown(); + // exit(1); + // } +} + +void ClusterLabelTool::setVisual(std::shared_ptr visual) +{ + // set new visual + m_visual = visual; + m_selectedFaces = visual->getFaces(); + m_faceSelectedArray.clear(); + for (auto faceId : m_selectedFaces) + { + if (m_faceSelectedArray.size() <= faceId) + { + m_faceSelectedArray.resize(faceId + 1); + } + m_faceSelectedArray[faceId] = true; + } +} + +void ClusterLabelTool::setSphereSize(float size) +{ + // m_clKernelSphere.setArg(3, size); + m_sphereSize = size; +} + +void ClusterLabelTool::activate() +{ +} + +void ClusterLabelTool::deactivate() +{ +} + +void ClusterLabelTool::setDisplay(ClusterLabelDisplay* display) +{ + m_display = display; + m_meshGeometry = m_display->getGeometry(); + m_faceSelectedArray.reserve(m_meshGeometry->faces.size()); + m_displayInitialized = true; + + m_vertexData.reserve(m_meshGeometry->faces.size() * 3 * 3); + + for (uint32_t faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) + { + for (uint32_t i = 0; i < 3; i++) + { + uint32_t vertexId = m_meshGeometry->faces[faceId].vertexIndices[i]; + Ogre::Vector3 vertexPos(m_meshGeometry->vertices[vertexId].x, m_meshGeometry->vertices[vertexId].y, + m_meshGeometry->vertices[vertexId].z); + m_vertexPositions.push_back(vertexPos); + + m_vertexData.push_back(m_meshGeometry->vertices[vertexId].x); + m_vertexData.push_back(m_meshGeometry->vertices[vertexId].y); + m_vertexData.push_back(m_meshGeometry->vertices[vertexId].z); + } + } + + // try-catch block to check for OpenCL errors + // try + // { + // m_clVertexBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY | CL_MEM_COPY_HOST_PTR, + // sizeof(float) * m_vertexData.size(), m_vertexData.data()); + + // m_clResultBuffer = cl::Buffer(m_clContext, CL_MEM_WRITE_ONLY | CL_MEM_HOST_READ_ONLY, + // sizeof(float) * m_meshGeometry->faces.size()); + + // m_clRayBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 6); + + // m_clSphereBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 4); + + // m_clBoxBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 4 * 6); + + // m_clStartNormalBuffer = cl::Buffer(m_clContext, CL_MEM_READ_ONLY | CL_MEM_HOST_WRITE_ONLY, sizeof(float) * 3); + + // m_clKernelSingleRay = cl::Kernel(m_clProgram, "cast_rays"); + // m_clKernelSphere = cl::Kernel(m_clProgram, "cast_sphere"); + // m_clKernelBox = cl::Kernel(m_clProgram, "cast_box"); + + // m_clKernelSingleRay.setArg(0, m_clVertexBuffer); + // m_clKernelSingleRay.setArg(1, m_clRayBuffer); + // m_clKernelSingleRay.setArg(2, m_clResultBuffer); + + // m_clKernelSphere.setArg(0, m_clVertexBuffer); + // m_clKernelSphere.setArg(1, m_clSphereBuffer); + // m_clKernelSphere.setArg(2, m_clResultBuffer); + // m_clKernelSphere.setArg(3, m_sphereSize); + + // m_clKernelBox.setArg(0, m_clVertexBuffer); + // m_clKernelBox.setArg(1, m_clBoxBuffer); + // m_clKernelBox.setArg(2, m_clResultBuffer); + // } + // catch (cl::Error err) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + // rclcpp::shutdown(); + // // ros::shutdown(); + // exit(1); + // } +} + +void ClusterLabelTool::updateSelectionBox() +{ + // TODO CHECK THIS + + // left = m_evt_start.x * 2 - 1; + // right = m_evt_stop.x * 2 - 1; + // top = 1 - m_evt_start.y * 2; + // bottom = 1 - m_evt_stop.y * 2; + + auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(m_evt_panel->getRenderWindow()); + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + + float m_bb_x1_rel = static_cast(m_bb_x1) / static_cast(width); + float m_bb_x2_rel = static_cast(m_bb_x2) / static_cast(width); + float m_bb_y1_rel = static_cast(m_bb_y1) / static_cast(height); + float m_bb_y2_rel = static_cast(m_bb_y2) / static_cast(height); + + float left = m_bb_x1_rel * 2.0 - 1.0; + float right = m_bb_x2_rel * 2.0 - 1.0; + float top = 1.0 - m_bb_y1_rel * 2.0; + float bottom = 1.0 - m_bb_y2_rel * 2.0; + + m_selectionBox->clear(); + m_selectionBox->begin(m_selectionBoxMaterial->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); + m_selectionBox->position(left, top, -1); + m_selectionBox->position(right, top, -1); + m_selectionBox->position(right, bottom, -1); + m_selectionBox->position(left, bottom, -1); + m_selectionBox->triangle(0, 1, 2); + m_selectionBox->triangle(0, 2, 3); + m_selectionBox->end(); + + +} + +void ClusterLabelTool::selectionBoxStart(rviz_common::ViewportMouseEvent& event) +{ + // OLD: doesnt work anymore + // m_selectionStart.x = (float)event.x / event.viewport->getActualWidth(); + // m_selectionStart.y = (float)event.y / event.viewport->getActualHeight(); + // NEW: + // TODO: Check if this is right + // m_selectionStart.x = (float)event.x / event.panel->getRenderWindow()->width(); + // m_selectionStart.y = (float)event.y / event.panel->getRenderWindow()->height(); + + m_bb_x1 = event.x; + m_bb_y1 = event.y; + m_evt_panel = event.panel; + + m_bb_x2 = m_bb_x1; + m_bb_y2 = m_bb_y1; + m_selectionBox->clear(); + m_selectionBox->setVisible(true); +} + +void ClusterLabelTool::selectionBoxMove(rviz_common::ViewportMouseEvent& event) +{ + // m_selectionStop.x = (float)event.x / event.viewport->getActualWidth(); + // m_selectionStop.y = (float)event.y / event.viewport->getActualHeight(); + + // Not possible + // m_selectionStop.x = (float)event.x / event.panel->getRenderWindow()->width(); + // m_selectionStop.y = (float)event.y / event.panel->getRenderWindow()->height(); + + + m_bb_x2 = event.x; + m_bb_y2 = event.y; + m_evt_panel = event.panel; + + updateSelectionBox(); +} + +void ClusterLabelTool::selectMultipleFaces( + rviz_common::ViewportMouseEvent& event, + bool selectMode) +{ + m_selectionBox->setVisible(false); + + int left = m_bb_x1; + int right = m_bb_x2; + int top = m_bb_y1; + int bottom = m_bb_y2; + + size_t goalSection; + size_t goalIndex; + + if (left > right) + { + std::swap(left, right); + } + + if (top > bottom) + { + std::swap(top, bottom); + } + + const int BOX_SIZE_TOLERANCE = 1; + if ((right - left) * (bottom - top) < BOX_SIZE_TOLERANCE) + { + // single face + selectSingleFace(event, selectMode); + } else { + // std::cout << "PICK!" << std::endl; + rviz_common::interaction::M_Picked pick_results; + + auto manager = context_->getSelectionManager(); + + manager->pick( + m_evt_panel->getRenderWindow(), + m_bb_x1, m_bb_y1, + m_bb_x2, m_bb_y2, + pick_results); + + if(!pick_results.empty()) + { + // FOUND SOMETHING! + // std::cout << "FOUND SOMETHING! " << pick_results.size() << std::endl; + + for(auto elem : pick_results) + { + rviz_common::interaction::CollObjectHandle key = elem.first; + rviz_common::interaction::Picked value = elem.second; + + // std::cout << key << std::endl; + // std::cout << "Extra handles: " << value.extra_handles.size() << std::endl; + + } + + // TODO: continue implementing this + } else { + // std::cout << "NOTHING to pick :(" << std::endl; + } + } + + // Ogre::PlaneBoundedVolume volume = + // event.panel->getViewController()->getCamera()->getCameraToViewportBoxVolume(left, top, right, bottom, true); + + // selectFacesInBoxParallel(volume, selectMode); + + // auto manager = context_->getSelectionManager(); + + // virtual void SelectionManager::pick( + // rviz_rendering::RenderWindow * window, + // int x1, + // int y1, + // int x2, + // int y2, + // M_Picked & results) = 0; + + +} + +void ClusterLabelTool::selectFacesInBoxParallel(Ogre::PlaneBoundedVolume& volume, bool selectMode) +{ + m_boxData.clear(); + for (Ogre::Plane plane : volume.planes) + { + m_boxData.push_back(plane.normal.x); + m_boxData.push_back(plane.normal.y); + m_boxData.push_back(plane.normal.z); + m_boxData.push_back(plane.d); + } + + // try + // { + // m_clQueue.enqueueWriteBuffer(m_clBoxBuffer, CL_TRUE, 0, sizeof(float) * 4 * 6, m_boxData.data()); + + // m_clQueue.enqueueNDRangeKernel(m_clKernelBox, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), + // cl::NullRange, nullptr); + // m_clQueue.finish(); + + // m_resultDistances.resize(m_meshGeometry->faces.size()); + // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), + // m_resultDistances.data()); + // } + // catch (cl::Error err) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } + + for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) + { + if (m_resultDistances[faceId] > 0) + { + if (m_faceSelectedArray.size() <= faceId) + { + m_faceSelectedArray.resize(faceId + 1); + } + m_faceSelectedArray[faceId] = selectMode; + } + } + + std::vector tmpFaceList; + + for (uint32_t faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + tmpFaceList.push_back(faceId); + } + } + + if (m_displayInitialized && m_visual) + { + m_visual->setFacesInCluster(tmpFaceList); + // m_visual->setColor(Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f)); + } +} + +void ClusterLabelTool::selectSingleFace( + rviz_common::ViewportMouseEvent& event, + bool selectMode) +{ + + Ogre::Ray mouse_ray = getMouseEventRay(event); + + Intersection intersection; + if(selectFace(context_, mouse_ray, intersection)) + { + // std::cout << "selectSingleFace- HIT!" << std::endl; + + if (m_displayInitialized && m_visual) + { + + std::vector tmpFaceList; + if(m_faceSelectedArray.size() <= intersection.face_id) + { + // TODO: what is this? wtf + m_faceSelectedArray.resize(intersection.face_id + 1); + } + m_faceSelectedArray[intersection.face_id] = selectMode; + + for(int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + tmpFaceList.push_back(faceId); + } + } + + m_visual->setFacesInCluster(tmpFaceList); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "selectSingleFaceParallel() found face with id %d", intersection.face_id); + } + + } else { + // std::cout << "selectSingleFace - No hit :(" << std::endl; + } +} + +void ClusterLabelTool::selectSingleFaceParallel(Ogre::Ray& ray, bool selectMode) +{ + m_rayData = { ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z, + ray.getDirection().x, ray.getDirection().y, ray.getDirection().z }; + + std::vector> intersectedFaceList; + + // try + // { + // m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); + + // m_clQueue.enqueueNDRangeKernel(m_clKernelSingleRay, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), + // cl::NullRange, nullptr); + // m_clQueue.finish(); + + // m_resultDistances.resize(m_meshGeometry->faces.size()); + // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), + // m_resultDistances.data()); + // } + // catch (cl::Error err) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } + + int closestFaceId = -1; + float minDist = std::numeric_limits::max(); + + for (int i = 0; i < m_meshGeometry->faces.size(); i++) + { + if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) + { + closestFaceId = i; + minDist = m_resultDistances[i]; + } + } + + if (m_displayInitialized && m_visual && closestFaceId != -1) + { + std::vector tmpFaceList; + + if (m_faceSelectedArray.size() <= closestFaceId) + { + m_faceSelectedArray.resize(closestFaceId + 1); + } + m_faceSelectedArray[closestFaceId] = selectMode; + + for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + tmpFaceList.push_back(faceId); + } + } + + m_visual->setFacesInCluster(tmpFaceList); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "selectSingleFaceParallel() found face with id %d", closestFaceId); + } +} + +void ClusterLabelTool::selectSphereFaces( + rviz_common::ViewportMouseEvent& event, bool selectMode) +{ + // Ogre::Ray ray = event.viewport->getCamera()->getCameraToViewportRay( + // (float)event.x / event.viewport->getActualWidth(), (float)event.y / event.viewport->getActualHeight()); + // Ogre::Ray ray = event.panel->getViewController()->getCamera()->getCameraToViewportRay( + // (float)event.x / event.panel->getRenderWindow()->width(), + // (float)event.y / event.panel->getRenderWindow()->height() + // ); + throw std::runtime_error("TODO"); + Ogre::Ray ray; + selectSphereFacesParallel(ray, selectMode); +} + +void ClusterLabelTool::selectSphereFacesParallel( + Ogre::Ray& ray, bool selectMode) +{ + auto raycastResult = getClosestIntersectedFaceParallel(ray); + + if (raycastResult) + { + Ogre::Vector3 sphereCenter = ray.getPoint(raycastResult->second); + + m_sphereData = { sphereCenter.x, sphereCenter.y, sphereCenter.z, raycastResult->second }; + + // try + // { + // m_clQueue.enqueueWriteBuffer(m_clSphereBuffer, CL_TRUE, 0, sizeof(float) * 4, m_sphereData.data()); + + // m_clQueue.enqueueNDRangeKernel(m_clKernelSphere, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), + // cl::NullRange, nullptr); + // m_clQueue.finish(); + + // m_resultDistances.resize(m_meshGeometry->faces.size()); + // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), + // m_resultDistances.data()); + // } + // catch (cl::Error err) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } + + for (int faceId = 0; faceId < m_meshGeometry->faces.size(); faceId++) + { + // if face is inside sphere, select it + if (m_resultDistances[faceId] > 0) + { + if (m_faceSelectedArray.size() <= faceId) + { + m_faceSelectedArray.resize(faceId + 1); + } + m_faceSelectedArray[faceId] = selectMode; + } + } + + if (m_displayInitialized && m_visual) + { + std::vector tmpFaceList; + for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + tmpFaceList.push_back(faceId); + } + } + + m_visual->setFacesInCluster(tmpFaceList); + } + } +} + +boost::optional> ClusterLabelTool::getClosestIntersectedFaceParallel( + Ogre::Ray& ray) +{ + m_rayData = { ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z, + ray.getDirection().x, ray.getDirection().y, ray.getDirection().z }; + + // try + // { + // m_clQueue.enqueueWriteBuffer(m_clRayBuffer, CL_TRUE, 0, sizeof(float) * 6, m_rayData.data()); + + // m_clQueue.enqueueNDRangeKernel(m_clKernelSingleRay, cl::NullRange, cl::NDRange(m_meshGeometry->faces.size()), + // cl::NullRange, nullptr); + // m_clQueue.finish(); + + // m_resultDistances.resize(m_meshGeometry->faces.size()); + // m_clQueue.enqueueReadBuffer(m_clResultBuffer, CL_TRUE, 0, sizeof(float) * m_meshGeometry->faces.size(), + // m_resultDistances.data()); + // } + // catch (cl::Error err) + // { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), err.what() << ": " << CLUtil::getErrorString(err.err())); + // RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "(" << CLUtil::getErrorDescription(err.err()) << ")"); + // } + + uint32_t closestFaceId; + bool faceFound = false; + float minDist = std::numeric_limits::max(); + + for (uint32_t i = 0; i < m_meshGeometry->faces.size(); i++) + { + if (m_resultDistances[i] > 0 && m_resultDistances[i] < minDist) + { + closestFaceId = i; + faceFound = true; + minDist = m_resultDistances[i]; + } + } + + if (faceFound) + { + return std::make_pair(closestFaceId, minDist); + } + else + { + return {}; + } +} + +void ClusterLabelTool::publishLabel(std::string label) +{ + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Label Tool: Publish label '" << label << "'"); + + vector faces; + for (uint32_t i = 0; i < m_faceSelectedArray.size(); i++) + { + if (m_faceSelectedArray[i]) + faces.push_back(i); + } + + m_display->addLabel(label, faces); +} + +// Handling mouse event and mark the clicked faces +int ClusterLabelTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) +{ + if (event.leftDown() && event.control()) + { + m_singleSelect = true; + selectSphereFaces(event, true); + } + else if (event.leftUp() && m_singleSelect) + { + m_singleSelect = false; + selectSphereFaces(event, true); + } + else if (event.rightDown() && event.control()) + { + m_singleDeselect = true; + selectSphereFaces(event, false); + } + else if (event.rightUp() && m_singleDeselect) + { + m_singleDeselect = false; + selectSphereFaces(event, false); + } + else if (event.leftDown()) + { + m_multipleSelect = true; + selectionBoxStart(event); + } + else if (event.leftUp() && m_multipleSelect) + { + m_multipleSelect = false; + selectMultipleFaces(event, true); + } + else if (event.rightDown()) + { + m_multipleSelect = true; + selectionBoxStart(event); + } + else if (event.rightUp() && m_multipleSelect) + { + m_multipleSelect = false; + selectMultipleFaces(event, false); + } + else if (m_multipleSelect) + { + selectionBoxMove(event); + } + else if (m_singleSelect) + { + selectSphereFaces(event, true); + } + else if (m_singleDeselect) + { + selectSphereFaces(event, false); + } + + return Render; +} + +std::vector ClusterLabelTool::getSelectedFaces() +{ + std::vector faceList; + + for (int faceId = 0; faceId < m_faceSelectedArray.size(); faceId++) + { + if (m_faceSelectedArray[faceId]) + { + faceList.push_back(faceId); + } + } + return faceList; +} + +void ClusterLabelTool::resetFaces() +{ + m_faceSelectedArray.clear(); + if(m_visual) + { + m_visual->setFacesInCluster(std::vector()); + } +} + +void ClusterLabelTool::resetVisual() +{ + // TODO: Segfault here, when using RViz config + m_visual.reset(); +} + +} // End namespace rviz_mesh_tools_plugins + + + +#include +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::ClusterLabelTool, rviz_common::Tool) \ No newline at end of file diff --git a/rviz_map_plugin/src/ClusterLabelVisual.cpp b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp similarity index 75% rename from rviz_map_plugin/src/ClusterLabelVisual.cpp rename to rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp index ff7c611..2b5c17f 100644 --- a/rviz_map_plugin/src/ClusterLabelVisual.cpp +++ b/rviz_mesh_tools_plugins/src/ClusterLabelVisual.cpp @@ -45,29 +45,38 @@ * Kristin Schmidt */ -#include +#include -#include +#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace rviz_map_plugin +namespace rviz_mesh_tools_plugins { -ClusterLabelVisual::ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId) - : m_displayContext(context), m_labelId(labelId) +// ClusterLabelVisual::ClusterLabelVisual( +// rviz_common::DisplayContext* context, +// std::string labelId) +// :m_displayContext(context) +// ,m_labelId(labelId) +// { +// } + +ClusterLabelVisual::ClusterLabelVisual( + rviz_common::DisplayContext* context, + std::string labelId, + std::shared_ptr geometry) +:m_displayContext(context) +,m_labelId(labelId) +,m_geometry(geometry) { -} -ClusterLabelVisual::ClusterLabelVisual(rviz::DisplayContext* context, std::string labelId, - std::shared_ptr geometry) - : m_displayContext(context), m_labelId(labelId), m_geometry(geometry) -{ // Get or create scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); Ogre::SceneNode* rootNode = sceneManager->getRootSceneNode(); @@ -87,7 +96,7 @@ ClusterLabelVisual::ClusterLabelVisual(rviz::DisplayContext* context, std::strin // Retrieve or create the mesh and attach it to the scene node m_mesh = Ogre::MeshManager::getSingleton().getByName("ClusterLabelMesh", "General"); - if (m_mesh.isNull() && geometry) + if(m_mesh.isNull() && geometry) { m_mesh = Ogre::MeshManager::getSingleton().createManual("ClusterLabelMesh", "General"); @@ -133,19 +142,28 @@ ClusterLabelVisual::ClusterLabelVisual(rviz::DisplayContext* context, std::strin // Create an entity of the mesh and add it to the scene Ogre::Entity* entity = sceneManager->createEntity("ClusterLabelEntity", "ClusterLabelMesh", "General"); + if(!entity) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "nullptr return: sceneManager->createEntity(\"ClusterLabelEntity\", \"ClusterLabelMesh\", \"General\"); "); + } entity->setMaterialName("CustomMaterial", "General"); m_sceneNode->attachObject(entity); } // Create a submesh and a custom material for it - if (!m_mesh.isNull()) + if(!m_mesh.isNull()) { m_subMesh = m_mesh->createSubMesh(m_labelId); m_subMesh->useSharedVertices = true; std::stringstream sstm; sstm << "ClusterLabel_Material_" << m_labelId; - m_material = Ogre::MaterialManager::getSingleton().create( + + m_material = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + if(m_material.isNull()) + { + m_material = Ogre::MaterialManager::getSingleton().create( sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); + } m_subMesh->setMaterialName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); m_material->getTechnique(0)->removeAllPasses(); @@ -163,7 +181,7 @@ ClusterLabelVisual::~ClusterLabelVisual() if (!m_mesh.isNull()) { - ROS_DEBUG("ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Destroying SubMesh: %s", m_labelId.c_str()); try { @@ -171,23 +189,32 @@ ClusterLabelVisual::~ClusterLabelVisual() } catch (...) { - ROS_ERROR("Exception in Visual destructor"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Exception in Visual destructor"); } } if (m_sceneNode->numAttachedObjects() == 0) { - ROS_INFO("ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::~ClusterLabelVisual: Delete scene node"); m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode); } + } void ClusterLabelVisual::reset() { - if (!m_material.isNull()) + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Reset ClusterLabelVisual"); + if(!m_material.isNull()) { - Ogre::MaterialManager::getSingleton().unload(m_material->getName()); - Ogre::MaterialManager::getSingleton().remove(m_material->getName()); + // if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(m_material->getName(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) + // { + m_material->unload(); + Ogre::MaterialManager::getSingleton().remove(m_material); + // } else { + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << m_material->getName() << "' to unload. skipping"); + // } + } else { + } } @@ -199,9 +226,9 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) { m_faces = faces; - if (!m_geometry) + if(!m_geometry) { - ROS_WARN("ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: MeshGeometry not set!"); return; } @@ -212,7 +239,7 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) m_subMesh->indexData->indexCount = 0; m_subMesh->indexData->indexStart = 0; m_material->getTechnique(0)->removeAllPasses(); - ROS_DEBUG("ClusterLabelVisual::setFacesInCluster: faces empty!"); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "ClusterLabelVisual::setFacesInCluster: faces empty!"); return; } @@ -222,7 +249,6 @@ void ClusterLabelVisual::setFacesInCluster(const std::vector& faces) m_material->getTechnique(0)->createPass(); m_material->setDiffuse(m_color); m_material->setSelfIllumination(m_color); - initMaterial(); } @@ -271,4 +297,4 @@ void ClusterLabelVisual::initMaterial() m_material->setDepthWriteEnabled(false); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/src/InteractionHelper.cpp b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp new file mode 100644 index 0000000..9afbdaf --- /dev/null +++ b/rviz_mesh_tools_plugins/src/InteractionHelper.cpp @@ -0,0 +1,180 @@ +#include + +#include +#include +#include + +#include +#include +#include + +namespace rviz_mesh_tools_plugins +{ + +Ogre::Ray getMouseEventRay( + const rviz_common::ViewportMouseEvent& event) +{ + auto viewport = rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()); + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay( + static_cast(event.x) / static_cast(width), + static_cast(event.y) / static_cast(height)); + return mouse_ray; +} + + +void getRawManualObjectData( + const Ogre::ManualObject* mesh, + const size_t sectionNumber, + size_t& vertexCount, Ogre::Vector3*& vertices, + size_t& indexCount, unsigned long*& indices) +{ + Ogre::VertexData* vertexData; + const Ogre::VertexElement* vertexElement; + Ogre::HardwareVertexBufferSharedPtr vertexBuffer; + unsigned char* vertexChar; + float* vertexFloat; + + vertexData = mesh->getSection(sectionNumber)->getRenderOperation()->vertexData; + vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION); + vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource()); + vertexChar = static_cast(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); + + vertexCount = vertexData->vertexCount; + vertices = new Ogre::Vector3[vertexCount]; + + for (size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize()) + { + vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat); + vertices[i] = + (mesh->getParentNode()->_getDerivedOrientation() * + (Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]) * mesh->getParentNode()->_getDerivedScale())) + + mesh->getParentNode()->_getDerivedPosition(); + } + + vertexBuffer->unlock(); + + Ogre::IndexData* indexData; + Ogre::HardwareIndexBufferSharedPtr indexBuffer; + indexData = mesh->getSection(sectionNumber)->getRenderOperation()->indexData; + indexCount = indexData->indexCount; + indices = new unsigned long[indexCount]; + indexBuffer = indexData->indexBuffer; + unsigned int* pLong = static_cast(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY)); + unsigned short* pShort = reinterpret_cast(pLong); + + for (size_t i = 0; i < indexCount; i++) + { + unsigned long index; + if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT) + { + index = static_cast(pLong[i]); + } + else + { + index = static_cast(pShort[i]); + } + + indices[i] = index; + } + indexBuffer->unlock(); +} + +bool selectFace( + const Ogre::ManualObject* mesh, + const Ogre::Ray& ray, + Intersection& intersection) +{ + Ogre::Real dist = -1.0f; + Ogre::Vector3 a, b, c; + + size_t vertex_count = 0; + Ogre::Vector3* vertices; + size_t index_count = 0; + unsigned long* indices; + size_t num_sections = mesh->getNumSections(); + + for (size_t i = 0; i < num_sections; i++) + { + getRawManualObjectData(mesh, i, vertex_count, vertices, index_count, indices); + if (index_count != 0) + { + for (size_t face_id = 0; face_id < index_count / 3; face_id++) + { + // face: indices[j], + const Ogre::Vector3 vertex; + + std::pair goal = Ogre::Math::intersects(ray, + vertices[indices[face_id * 3 + 0]], + vertices[indices[face_id * 3 + 1]], + vertices[indices[face_id * 3 + 2]], true, true); + + if (goal.first) + { + if ((dist < 0.0f) || (goal.second < dist)) + { + dist = goal.second; + a = vertices[indices[face_id * 3 + 0]]; + b = vertices[indices[face_id * 3 + 1]]; + c = vertices[indices[face_id * 3 + 2]]; + intersection.face_id = face_id; + } + } + } + } + } + + delete[] vertices; + delete[] indices; + if (dist != -1) + { + intersection.range = dist; + intersection.point = ray.getPoint(dist); + Ogre::Vector3 ab = b - a; + Ogre::Vector3 ac = c - a; + intersection.normal = ac.crossProduct(ab).normalisedCopy(); + return true; + } + else + { + return false; + } +} + +bool selectFace( + Ogre::SceneManager* sm, + const Ogre::Ray& ray, + Intersection& intersection) +{ + Ogre::RaySceneQuery* query = sm->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK); + query->setSortByDistance(true); + + Ogre::RaySceneQueryResult& result = query->execute(); + + for (size_t i = 0; i < result.size(); i++) + { + if (result[i].movable->getName().find("TriangleMesh") != std::string::npos) + { + Ogre::ManualObject* mesh = static_cast(result[i].movable); + size_t goal_section = -1; + size_t goal_index = -1; + Ogre::Real dist = -1; + if (selectFace(mesh, ray, intersection)) + { + return true; + } + } + } + return false; +} + +bool selectFace( + const rviz_common::DisplayContext* ctx, + const Ogre::Ray& ray, + Intersection& intersection) +{ + return selectFace(ctx->getSceneManager(), ray, intersection); +} + +} // namespace rviz_mesh_tools_plugins \ No newline at end of file diff --git a/rviz_mesh_tools_plugins/src/MapDisplay.cpp b/rviz_mesh_tools_plugins/src/MapDisplay.cpp new file mode 100644 index 0000000..7e3c9cf --- /dev/null +++ b/rviz_mesh_tools_plugins/src/MapDisplay.cpp @@ -0,0 +1,599 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * MapDisplay.cpp + * + * + * authors: + * + * Kristin Schmidt + * Jan Philipp Vogtherr + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// #include +// #include +#include +#include + +#include +#include +#include + + +namespace rviz_mesh_tools_plugins +{ +MapDisplay::MapDisplay() +:m_clusterLabelDisplay(nullptr) +,m_meshDisplay(nullptr) +{ + m_mapFilePath = new rviz_common::properties::FileProperty("Map file path", "/path/to/map.h5", "Absolute path of the map file", this, + SLOT(updateMap())); +} + +MapDisplay::~MapDisplay() +{ +} + +// ===================================================================================================================== +// Public Q_SLOTS + +std::shared_ptr MapDisplay::getGeometry() +{ + if (!m_geometry) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Geometry requested, but none available!"); + } + return m_geometry; +} + +// ===================================================================================================================== +// Callbacks + +rviz_common::Display* MapDisplay::createDisplay(const QString& class_id) +{ + rviz_common::Display* disp = context_->getRootDisplayGroup()->createDisplay(class_id); + + if (!disp) + { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "IM SEARCHING FOR rviz_common::FailedDisplay"); + } + return disp; +} + +void MapDisplay::enableClusterLabelDisplay() +{ + if(!m_clusterLabelDisplay) + { + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); + Display* display = createDisplay("rviz_mesh_tools_plugins/ClusterLabel"); + if (m_clusterLabelDisplay = dynamic_cast(display); m_clusterLabelDisplay != nullptr) + { + m_clusterLabelDisplay->setName("ClusterLabel"); + m_clusterLabelDisplay->setModel(model_); + m_clusterLabelDisplay->setParent(this); + addChild(m_clusterLabelDisplay); + m_clusterLabelDisplay->initialize(context_); + + // Make signal/slot connections + connect(m_clusterLabelDisplay, SIGNAL(signalAddLabel(Cluster)), this, SLOT(saveLabel(Cluster))); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. CREATED"); + } else { + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel. NOT FOUND"); + } + } else { + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "rviz_mesh_tools_plugins/ClusterLabel. ALREADY EXISTING"); + m_clusterLabelDisplay->onEnable(); + m_clusterLabelDisplay->show(); + } +} + +void MapDisplay::disableClusterLabelDisplay() +{ + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->onDisable(); + m_clusterLabelDisplay->hide(); + } +} + +void MapDisplay::enableMeshDisplay() +{ + if(!m_meshDisplay) + { + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/Mesh"); + Display* meshDisplay = createDisplay("rviz_mesh_tools_plugins/Mesh"); + if(m_meshDisplay = dynamic_cast(meshDisplay); m_meshDisplay != nullptr) + { + addChild(m_meshDisplay); + m_meshDisplay->setName("Mesh"); + m_meshDisplay->setModel(model_); + m_meshDisplay->setParent(this); + m_meshDisplay->initialize(context_); + m_meshDisplay->ignoreIncomingMessages(); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/Mesh. CREATED"); + } else { + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/Mesh. NOT FOUND"); + } + } else { + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "rviz_mesh_tools_plugins/Mesh. ALREADY EXISTING"); + m_meshDisplay->onEnable(); + m_meshDisplay->show(); + } +} + +void MapDisplay::disableMeshDisplay() +{ + if(m_meshDisplay) + { + m_meshDisplay->onDisable(); + m_meshDisplay->hide(); + } +} + +void MapDisplay::onInitialize() +{ + std::string name = this->getName().toStdString(); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "createDisplay: rviz_mesh_tools_plugins/ClusterLabel"); +} + +void MapDisplay::onEnable() +{ + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->onEnable(); + } + + if(m_meshDisplay) + { + m_meshDisplay->onEnable(); + } +} + +void MapDisplay::onDisable() +{ + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->onDisable(); + } + if(m_meshDisplay) + { + m_meshDisplay->onDisable(); + } + +} + +// ===================================================================================================================== +// Callbacks triggered from UI events (mostly) + +void MapDisplay::load(const rviz_common::Config& config) +{ + std::string name = this->getName().toStdString(); + std::cout << name << ": LOAD CONFIG..." << std::endl; + + rviz_common::Config config2 = config; + + { // Override with ros params + std::stringstream ss; + ss << "rviz_mesh_tools_plugins/" << name; + + std::string mesh_file; + + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + if(node->get_parameter(ss.str(), mesh_file)) + { + config2.mapSetValue(m_mapFilePath->getName(), QString::fromStdString(mesh_file) ); + } else { + std::cout << name << ": COULDN'T FIND MESH TO LOAD" << std::endl; + } + } + + rviz_common::Display::load(config2); + + std::cout << name << ": LOAD CONFIG done." << std::endl; +} + +void MapDisplay::updateMap() +{ + std::string name = this->getName().toStdString(); + std::cout << name << ": updateMap" << std::endl; + + // Load geometry and clusters + bool successful = loadData(); + if (!successful) + { + return; + } + + if(m_meshDisplay) + { + // Update sub-plugins + m_meshDisplay->setGeometry(m_geometry); + m_meshDisplay->setVertexColors(m_colors); + m_meshDisplay->setVertexNormals(m_normals); + m_meshDisplay->clearVertexCosts(); + for (const auto& vertexCosts : m_costs) + { + std::vector costs = vertexCosts.second; + m_meshDisplay->addVertexCosts(vertexCosts.first, costs); + } + m_meshDisplay->setMaterials(m_materials, m_texCoords); + // m_meshDisplay->setTexCoords(m_texCoords); + for (uint32_t i = 0; i < m_textures.size(); i++) + { + m_meshDisplay->addTexture(m_textures[i], i); + } + } + + if(m_clusterLabelDisplay) + { + m_clusterLabelDisplay->setData(m_geometry, m_clusterList); + } + + + // All good + setStatus(rviz_common::properties::StatusProperty::Ok, "Map", ""); + + m_map_file_loaded = m_mapFilePath->getFilename(); +} + +// ===================================================================================================================== +// Data loading + +bool MapDisplay::loadData() +{ + std::string name = this->getName().toStdString(); + + if(m_mapFilePath->getFilename() == m_map_file_loaded) + { + std::cout << name << "! Tried to load same map twice. Skipping and keeping old data" << std::endl; + return true; + } + + // Read map file path + std::string mapFile = m_mapFilePath->getFilename(); + if (mapFile.empty()) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: No map file path specified!"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "No map file path specified!"); + return false; + } + if (!boost::filesystem::exists(mapFile)) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Specified map file does not exist!"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Map", "Specified map file does not exist!"); + return false; + } + + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Loading data for map '" << mapFile << "'"); + + try + { + if (boost::filesystem::extension(mapFile).compare(".h5") == 0) + { + enableClusterLabelDisplay(); // enable label writing to hdf5 + enableMeshDisplay(); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load HDF5 map"); + // Open file IO + hdf5_map_io::HDF5MapIO map_io(mapFile); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load geometry"); + + // Read geometry + m_geometry = std::make_shared(Geometry(map_io.getVertices(), map_io.getFaceIds())); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load textures"); + + // Read textures + vector textures = map_io.getTextures(); + m_textures.resize(textures.size()); + for (size_t i = 0; i < textures.size(); i++) + { + // Find out the texture index because textures are not stored in ascending order + int textureIndex = std::stoi(textures[i].name); + + // Copy metadata + m_textures[textureIndex].width = textures[i].width; + m_textures[textureIndex].height = textures[i].height; + m_textures[textureIndex].channels = textures[i].channels; + m_textures[textureIndex].data = textures[i].data; + m_textures[textureIndex].pixelFormat = "rgb8"; + } + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load materials"); + + // Read materials + vector materials = map_io.getMaterials(); + vector faceToMaterialIndexArray = map_io.getMaterialFaceIndices(); + m_materials.resize(materials.size()); + for (size_t i = 0; i < materials.size(); i++) + { + // Copy material color + m_materials[i].color.r = materials[i].r / 255.0f; + m_materials[i].color.g = materials[i].g / 255.0f; + m_materials[i].color.b = materials[i].b / 255.0f; + m_materials[i].color.a = 1.0f; + + // Look for texture index + if (materials[i].textureIndex == -1) + { + // texture index -1: no texture + m_materials[i].textureIndex = boost::none; + } + else + { + m_materials[i].textureIndex = materials[i].textureIndex; + } + + m_materials[i].faceIndices.clear(); + } + + // Copy face indices + for (size_t k = 0; k < faceToMaterialIndexArray.size(); k++) + { + m_materials[faceToMaterialIndexArray[k]].faceIndices.push_back(k); + } + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex colors"); + + // Read vertex colors + vector colors = map_io.getVertexColors(); + m_colors.clear(); + m_colors.reserve(colors.size() / 3); + for (size_t i = 0; i < colors.size(); i += 3) + { + // convert from 0-255 (uint8) to 0.0-1.0 (float) + m_colors.push_back(Color(colors[i + 0] / 255.0f, colors[i + 1] / 255.0f, colors[i + 2] / 255.0f, 1.0)); + } + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load vertex normals"); + + // Read vertex normals + vector normals = map_io.getVertexNormals(); + m_normals.clear(); + m_normals.reserve(normals.size() / 3); + for (size_t i = 0; i < normals.size(); i += 3) + { + m_normals.push_back(Normal(normals[i + 0], normals[i + 1], normals[i + 2])); + } + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load texture coordinates"); + + // Read tex cords + vector texCoords = map_io.getVertexTextureCoords(); + m_texCoords.clear(); + m_texCoords.reserve(texCoords.size() / 3); + for (size_t i = 0; i < texCoords.size(); i += 3) + { + m_texCoords.push_back(TexCoords(texCoords[i], texCoords[i + 1])); + } + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Load clusters"); + + // Read labels + m_clusterList.clear(); + // m_clusterList.push_back(Cluster("__NEW__", vector())); + for (auto labelGroup : map_io.getLabelGroups()) + { + for (auto labelObj : map_io.getAllLabelsOfGroup(labelGroup)) + { + auto faceIds = map_io.getFaceIdsOfLabel(labelGroup, labelObj); + + std::stringstream ss; + ss << labelGroup << "_" << labelObj; + std::string label = ss.str(); + + m_clusterList.push_back(Cluster(label, faceIds)); + } + } + + m_costs.clear(); + for (std::string costlayer : map_io.getCostLayers()) + { + try + { + m_costs[costlayer] = map_io.getVertexCosts(costlayer); + } + catch (const hf::DataSpaceException& e) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load channel " << costlayer << " as a costlayer!"); + } + } + + + } + else + { + disableClusterLabelDisplay(); // we cannot write labels to standard formats + enableMeshDisplay(); + std::cout << "LOADING WITH ASSIMP" << std::endl; + // PLY, OBJ, DAE? -> ASSIMP + // The following lines are a simple way to import the mesh geometry + // of commonly used mesh file formats. + // + // TODOs: + // 1. scene graphs will not be imported properly. + // Someone has to do some transformations according to the + // node graph in the assimp structures. Or optionally (even better): + // create tf-transformations for every element of the scene graph# + // 2. HDF5 is used to store more information such as label etc. + // So we possibly need to transform the geometry from PLY, OBJ, DAE to H5 first?? + // + Assimp::Importer io; + io.SetPropertyBool(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, true); + + // with aiProcess_PreTransformVertices assimp transforms the whole scene graph + // into one mesh + // - if you want to use TF for spawning meshes, the loading has to be done manually + const aiScene* ascene = io.ReadFile(mapFile, aiProcess_PreTransformVertices); + // what if there is more than one mesh? + const aiMesh* amesh = ascene->mMeshes[0]; + + const aiVector3D* ai_vertices = amesh->mVertices; + const aiFace* ai_faces = amesh->mFaces; + + m_geometry = std::make_shared(); + + m_geometry->vertices.resize(amesh->mNumVertices); + m_geometry->faces.resize(amesh->mNumFaces); + + std::cout << "- Vertices, Faces: " << amesh->mNumVertices << ", " << amesh->mNumFaces << std::endl; + + for (int i = 0; i < amesh->mNumVertices; i++) + { + m_geometry->vertices[i].x = amesh->mVertices[i].x; + m_geometry->vertices[i].y = amesh->mVertices[i].y; + m_geometry->vertices[i].z = amesh->mVertices[i].z; + } + + for (int i = 0; i < amesh->mNumFaces; i++) + { + m_geometry->faces[i].vertexIndices[0] = amesh->mFaces[i].mIndices[0]; + m_geometry->faces[i].vertexIndices[1] = amesh->mFaces[i].mIndices[1]; + m_geometry->faces[i].vertexIndices[2] = amesh->mFaces[i].mIndices[2]; + } + + if(amesh->HasNormals()) + { + m_normals.resize(amesh->mNumVertices, Normal(0.0, 0.0, 0.0)); + for(int i=0; imNumVertices; i++) + { + m_normals[i].x = amesh->mNormals[i].x; + m_normals[i].y = amesh->mNormals[i].y; + m_normals[i].z = amesh->mNormals[i].z; + } + } else { + m_normals.resize(0, Normal(0.0, 0.0, 0.0)); + } + + // assimp supports more color channels but not this plugin + // can we support this too? + int color_channel_id = 0; + if(amesh->HasVertexColors(color_channel_id)) + { + m_colors.resize(amesh->mNumVertices, Color(0.0, 0.0, 0.0, 0.0)); + for(int i=0; imNumVertices; i++) + { + m_colors[i].r = amesh->mColors[color_channel_id][i].r; + m_colors[i].g = amesh->mColors[color_channel_id][i].g; + m_colors[i].b = amesh->mColors[color_channel_id][i].b; + m_colors[i].a = amesh->mColors[color_channel_id][i].a; + } + } else { + m_colors.resize(0, Color(0.0, 0.0, 0.0, 0.0)); + } + + m_costs.clear(); + + + } + } + catch (...) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "An unexpected error occurred while using Pluto Map IO"); + setStatus(rviz_common::properties::StatusProperty::Error, "IO", "An unexpected error occurred while using Pluto Map IO"); + return false; + } + + setStatus(rviz_common::properties::StatusProperty::Ok, "IO", ""); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Successfully loaded map."); + + return true; +} + +// ===================================================================================================================== +// Label + +void MapDisplay::saveLabel(Cluster cluster) +{ + std::string label = cluster.name; + std::vector faces = cluster.faces; + + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: add label '" << label << "'"); + + try + { + // Split label into class and instance (tree_1 => class "tree" & instance "1") + std::vector results; + boost::split(results, label, [](char c) { return c == '_'; }); + if (results.size() != 2) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Illegal label name '" << label << "'"); + setStatus(rviz_common::properties::StatusProperty::Error, "Label", "Illegal label name!"); + return; + } + + // Open IO + hdf5_map_io::HDF5MapIO map_io(m_mapFilePath->getFilename()); + + // Add label with faces list + map_io.addOrUpdateLabel(results[0], results[1], faces); + + // Add to cluster list + m_clusterList.push_back(Cluster(label, faces)); + + setStatus(rviz_common::properties::StatusProperty::Ok, "Label", "Successfully saved label"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Map Display: Successfully added label to map."); + + // update the map to show the new label + updateMap(); + } + catch (...) + { + setStatus(rviz_common::properties::StatusProperty::Error, "Label", "Error while saving label"); + } +} + +} // End namespace rviz_mesh_tools_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MapDisplay, rviz_common::Display) diff --git a/rviz_map_plugin/src/MeshDisplay.cpp b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp similarity index 50% rename from rviz_map_plugin/src/MeshDisplay.cpp rename to rviz_mesh_tools_plugins/src/MeshDisplay.cpp index f6ea39f..0a9ceb4 100644 --- a/rviz_map_plugin/src/MeshDisplay.cpp +++ b/rviz_mesh_tools_plugins/src/MeshDisplay.cpp @@ -51,38 +51,59 @@ * Malte kleine Piening */ -#include +#include -#include -#include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include -#include -#include -#include -namespace rviz_map_plugin +#include "mesh_msgs/srv/get_vertex_colors.hpp" +#include "mesh_msgs/srv/get_materials.hpp" +#include "mesh_msgs/srv/get_geometry.hpp" +#include "mesh_msgs/srv/get_texture.hpp" +#include "mesh_msgs/srv/get_uui_ds.hpp" + +#include "mesh_msgs/msg/mesh_vertex_colors_stamped.hpp" +#include "mesh_msgs/msg/mesh_geometry_stamped.hpp" + + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "rclcpp/executor.hpp" +#include "rmw/validate_full_topic_name.h" + + +using namespace std::chrono_literals; +using std::placeholders::_1; +using std::placeholders::_2; + +namespace rviz_mesh_tools_plugins { -MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) +MeshDisplay::MeshDisplay() +: rviz_common::Display() +, m_ignoreMsgs(false) { // mesh topic - m_meshTopic = new rviz::RosTopicProperty( - "Geometry Topic", "", QString::fromStdString(ros::message_traits::datatype()), + m_meshTopic = new rviz_common::properties::RosTopicProperty( + "Geometry Topic", "", QString::fromStdString(rosidl_generator_traits::name()), "Geometry topic to subscribe to.", this, SLOT(updateTopic())); // buffer size / amount of meshes visualized - m_bufferSize = new rviz::IntProperty("Buffer Size", 1, "Amount of meshes visualized", this, SLOT(updateBufferSize())); + m_bufferSize = new rviz_common::properties::IntProperty("Buffer Size", 1, "Number of meshes visualized", this, SLOT(updateBufferSize())); m_bufferSize->setMin(1); // Display Type { - m_displayType = new rviz::EnumProperty("Display Type", "Fixed Color", "Select Display Type for Mesh", this, + m_displayType = new rviz_common::properties::EnumProperty("Display Type", "Fixed Color", "Select Display Type for Mesh", this, SLOT(updateMesh()), this); m_displayType->addOption("Fixed Color", 0); m_displayType->addOption("Vertex Color", 1); @@ -93,11 +114,11 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Fixed Color { // face color properties - m_facesColor = new rviz::ColorProperty("Faces Color", QColor(0, 255, 0), "The color of the faces.", m_displayType, + m_facesColor = new rviz_common::properties::ColorProperty("Faces Color", QColor(0, 255, 0), "The color of the faces.", m_displayType, SLOT(updateMesh()), this); // face alpha properties - m_facesAlpha = new rviz::FloatProperty("Faces Alpha", 1.0, "The alpha-value of the faces", m_displayType, + m_facesAlpha = new rviz_common::properties::FloatProperty("Faces Alpha", 1.0, "The alpha-value of the faces", m_displayType, SLOT(updateMesh()), this); m_facesAlpha->setMin(0); m_facesAlpha->setMax(1); @@ -105,12 +126,12 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Vertex Color { - m_vertexColorsTopic = new rviz::RosTopicProperty( + m_vertexColorsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Colors Topic", "", - QString::fromStdString(ros::message_traits::datatype()), + QString::fromStdString(rosidl_generator_traits::name()), "Vertex color topic to subscribe to.", m_displayType, SLOT(updateVertexColorsTopic()), this); - m_vertexColorServiceName = new rviz::StringProperty("Vertex Color Service Name", "get_vertex_colors", + m_vertexColorServiceName = new rviz_common::properties::StringProperty("Vertex Color Service Name", "get_vertex_colors", "Name of the Vertex Color Service to request Vertex Colors " "from.", m_displayType, SLOT(updateVertexColorService()), this); @@ -118,48 +139,50 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Textures { - m_showTexturedFacesOnly = new rviz::BoolProperty("Show textured faces only", false, "Show textured faces only", + m_showTexturedFacesOnly = new rviz_common::properties::BoolProperty("Show textured faces only", false, "Show textured faces only", m_displayType, SLOT(updateMesh()), this); - m_materialServiceName = new rviz::StringProperty("Material Service Name", "get_materials", + m_materialServiceName = new rviz_common::properties::StringProperty("Material Service Name", "get_materials", "Name of the Matrial Service to request Materials from.", m_displayType, SLOT(updateMaterialAndTextureServices()), this); - m_textureServiceName = new rviz::StringProperty("Texture Service Name", "get_texture", + m_textureServiceName = new rviz_common::properties::StringProperty("Texture Service Name", "get_texture", "Name of the Texture Service to request Textures from.", m_displayType, SLOT(updateMaterialAndTextureServices()), this); } // Vertex Costs { - m_costColorType = new rviz::EnumProperty("Color Scale", "Rainbow", + m_costColorType = new rviz_common::properties::EnumProperty("Color Scale", "Rainbow", "Select color scale for vertex costs. Mesh will update when new data " "arrives.", m_displayType, SLOT(updateVertexCosts()), this); m_costColorType->addOption("Rainbow", 0); m_costColorType->addOption("Red Green", 1); - m_vertexCostsTopic = new rviz::RosTopicProperty( + + + m_vertexCostsTopic = new rviz_common::properties::RosTopicProperty( "Vertex Costs Topic", "", - QString::fromStdString(ros::message_traits::datatype()), + QString::fromStdString(rosidl_generator_traits::name()), "Vertex cost topic to subscribe to.", m_displayType, SLOT(updateVertexCostsTopic()), this); - m_selectVertexCostMap = new rviz::EnumProperty("Vertex Costs Type", "-- None --", + m_selectVertexCostMap = new rviz_common::properties::EnumProperty("Vertex Costs Type", "-- None --", "Select the type of vertex cost map to be displayed. New types " "will appear here when a new message arrives.", m_displayType, SLOT(updateVertexCosts()), this); m_selectVertexCostMap->addOption("-- None --", 0); - m_costUseCustomLimits = new rviz::BoolProperty("Use Custom limits", false, "Use custom vertex cost limits", + m_costUseCustomLimits = new rviz_common::properties::BoolProperty("Use Custom limits", false, "Use custom vertex cost limits", m_displayType, SLOT(updateVertexCosts()), this); // custom cost limits { - m_costLowerLimit = new rviz::FloatProperty("Vertex Costs Lower Limit", 0.0, "Vertex costs lower limit", + m_costLowerLimit = new rviz_common::properties::FloatProperty("Vertex Costs Lower Limit", 0.0, "Vertex costs lower limit", m_costUseCustomLimits, SLOT(updateVertexCosts()), this); m_costLowerLimit->hide(); - m_costUpperLimit = new rviz::FloatProperty("Vertex Costs Upper Limit", 1.0, "Vertex costs upper limit", + m_costUpperLimit = new rviz_common::properties::FloatProperty("Vertex Costs Upper Limit", 1.0, "Vertex costs upper limit", m_costUseCustomLimits, SLOT(updateVertexCosts()), this); m_costUpperLimit->hide(); } @@ -169,13 +192,13 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Wireframe { m_showWireframe = - new rviz::BoolProperty("Show Wireframe", true, "Show Wireframe", this, SLOT(updateWireframe()), this); + new rviz_common::properties::BoolProperty("Show Wireframe", true, "Show Wireframe", this, SLOT(updateWireframe()), this); // wireframe color property - m_wireframeColor = new rviz::ColorProperty("Wireframe Color", QColor(0, 0, 0), "The color of the wireframe.", + m_wireframeColor = new rviz_common::properties::ColorProperty("Wireframe Color", QColor(0, 0, 0), "The color of the wireframe.", m_showWireframe, SLOT(updateWireframe()), this); // wireframe alpha property - m_wireframeAlpha = new rviz::FloatProperty("Wireframe Alpha", 1.0, "The alpha-value of the wireframe", + m_wireframeAlpha = new rviz_common::properties::FloatProperty("Wireframe Alpha", 1.0, "The alpha-value of the wireframe", m_showWireframe, SLOT(updateWireframe()), this); m_wireframeAlpha->setMin(0); m_wireframeAlpha->setMax(1); @@ -183,54 +206,61 @@ MeshDisplay::MeshDisplay() : rviz::Display(), m_ignoreMsgs(false) // Normals { - m_showNormals = new rviz::BoolProperty("Show Normals", true, "Show Normals", this, SLOT(updateNormals()), this); + m_showNormals = new rviz_common::properties::BoolProperty("Show Normals", true, "Show Normals", this, SLOT(updateNormals()), this); - m_normalsColor = new rviz::ColorProperty("Normals Color", QColor(255, 0, 255), "The color of the normals.", + m_normalsColor = new rviz_common::properties::ColorProperty("Normals Color", QColor(255, 0, 255), "The color of the normals.", m_showNormals, SLOT(updateNormalsColor()), this); - m_normalsAlpha = new rviz::FloatProperty("Normals Alpha", 1.0, "The alpha-value of the normals", m_showNormals, + m_normalsAlpha = new rviz_common::properties::FloatProperty("Normals Alpha", 1.0, "The alpha-value of the normals", m_showNormals, SLOT(updateNormalsColor()), this); m_normalsAlpha->setMin(0); m_normalsAlpha->setMax(1); - m_scalingFactor = new rviz::FloatProperty("Normals Scaling Factor", 0.1, "Scaling factor of the normals", + m_scalingFactor = new rviz_common::properties::FloatProperty("Normals Scaling Factor", 0.1, "Scaling factor of the normals", m_showNormals, SLOT(updateNormalsSize()), this); } } MeshDisplay::~MeshDisplay() { + unsubscribe(); } void MeshDisplay::onInitialize() { - m_tfMeshFilter = new tf2_ros::MessageFilter( - *rviz::Display::context_->getTF2BufferPtr(), rviz::Display::fixed_frame_.toStdString(), 2, - rviz::Display::update_nh_); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + m_tfMeshFilter = std::make_shared >( + *context_->getFrameManager()->getTransformer(), rviz_common::Display::fixed_frame_.toStdString(), 2, + node); m_tfMeshFilter->connectInput(m_meshSubscriber); - context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfMeshFilter, this); + // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfMeshFilter, this); - m_tfVertexColorsFilter = new tf2_ros::MessageFilter( - *rviz::Display::context_->getTF2BufferPtr(), rviz::Display::fixed_frame_.toStdString(), 10, - rviz::Display::update_nh_); + m_tfVertexColorsFilter = std::make_shared >( + *context_->getFrameManager()->getTransformer(), rviz_common::Display::fixed_frame_.toStdString(), 10, + node); m_tfVertexColorsFilter->connectInput(m_vertexColorsSubscriber); - context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexColorsFilter, this); + // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexColorsFilter, this); - m_tfVertexCostsFilter = new tf2_ros::MessageFilter( - *rviz::Display::context_->getTF2BufferPtr(), rviz::Display::fixed_frame_.toStdString(), 10, - rviz::Display::update_nh_); + m_tfVertexCostsFilter = std::make_shared >( + *context_->getFrameManager()->getTransformer(), rviz_common::Display::fixed_frame_.toStdString(), 10, + node); m_tfVertexCostsFilter->connectInput(m_vertexCostsSubscriber); - context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexCostsFilter, this); + // context_->getFrameManager()->registerFilterForTransformStatusCheck(m_tfVertexCostsFilter, this); m_meshSynchronizer = 0; m_colorsSynchronizer = 0; m_costsSynchronizer = 0; // Initialize service clients - ros::NodeHandle n; - m_vertexColorClient = n.serviceClient(m_vertexColorServiceName->getStdString()); + // ros::NodeHandle n; + // m_node = rclcpp::Node::make_shared("mesh_display"); // TODO: how to name this? What if multiple MeshDisplay instances exists? - m_materialsClient = n.serviceClient(m_materialServiceName->getStdString()); + // auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - m_textureClient = n.serviceClient(m_textureServiceName->getStdString()); + m_vertexColorClient = node->create_client(m_vertexColorServiceName->getStdString()); + m_materialsClient = node->create_client(m_vertexColorServiceName->getStdString()); + m_textureClient = node->create_client(m_vertexColorServiceName->getStdString()); + m_uuidClient = node->create_client("get_uuid"); + m_geometryClient = node->create_client("get_geometry"); updateMesh(); updateWireframe(); @@ -264,14 +294,20 @@ void MeshDisplay::subscribe() try { - m_meshSubscriber.subscribe(update_nh_, m_meshTopic->getTopicStd(), 1); - m_vertexColorsSubscriber.subscribe(update_nh_, m_vertexColorsTopic->getTopicStd(), 1); - m_vertexCostsSubscriber.subscribe(update_nh_, m_vertexCostsTopic->getTopicStd(), 4); - setStatus(rviz::StatusProperty::Ok, "Topic", "OK"); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 1; + m_meshSubscriber.subscribe(node, m_meshTopic->getTopicStd(), qos); + qos.depth = 1; + m_vertexColorsSubscriber.subscribe(node, m_vertexColorsTopic->getTopicStd(), qos); + qos.depth = 4; + m_vertexCostsSubscriber.subscribe(node, m_vertexCostsTopic->getTopicStd(), qos); + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); } - catch (ros::Exception& e) + catch (std::runtime_error& e) { - setStatus(rviz::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what()); } // Nothing @@ -281,14 +317,14 @@ void MeshDisplay::subscribe() } else { - m_meshSynchronizer = new message_filters::Cache(m_meshSubscriber, 10); - m_meshSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingGeometry, this, _1)); + m_meshSynchronizer = new message_filters::Cache(m_meshSubscriber, 10); + m_meshSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingGeometry, this, _1)); - m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); - m_colorsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexColors, this, _1)); + m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); + m_colorsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexColors, this, _1)); - m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); - m_costsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexCosts, this, _1)); + m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); + m_costsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexCosts, this, _1)); } initialServiceCall(); @@ -344,7 +380,7 @@ void MeshDisplay::setGeometry(shared_ptr geometry) updateNormals(); updateWireframe(); } - setStatus(rviz::StatusProperty::Ok, "Display", ""); + setStatus(rviz_common::properties::StatusProperty::Ok, "Display", ""); } void MeshDisplay::setVertexColors(vector& vertexColors) @@ -424,7 +460,7 @@ void MeshDisplay::updateBufferSize() void MeshDisplay::updateMesh() { - ROS_INFO("Mesh Display: Update"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Mesh Display: Update"); bool showFaces = false; bool showTextures = false; @@ -509,7 +545,7 @@ void MeshDisplay::updateMesh() std::shared_ptr visual = getLatestVisual(); if (!visual) { - ROS_ERROR("Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Mesh display: no visual available, can't draw mesh! (maybe no data has been received yet?)"); return; } @@ -597,9 +633,13 @@ void MeshDisplay::updateVertexColorsTopic() m_vertexColorsSubscriber.unsubscribe(); delete m_colorsSynchronizer; - m_vertexColorsSubscriber.subscribe(update_nh_, m_vertexColorsTopic->getTopicStd(), 1); - m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); - m_colorsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexColors, this, _1)); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 1; + m_vertexColorsSubscriber.subscribe(node, m_vertexColorsTopic->getTopicStd(), qos); + m_colorsSynchronizer = new message_filters::Cache(m_vertexColorsSubscriber, 1); + m_colorsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexColors, this, _1)); } void MeshDisplay::updateVertexCostsTopic() @@ -607,9 +647,13 @@ void MeshDisplay::updateVertexCostsTopic() m_vertexCostsSubscriber.unsubscribe(); delete m_costsSynchronizer; - m_vertexCostsSubscriber.subscribe(update_nh_, m_vertexCostsTopic->getTopicStd(), 4); - m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); - m_costsSynchronizer->registerCallback(boost::bind(&MeshDisplay::incomingVertexCosts, this, _1)); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 4; + m_vertexCostsSubscriber.subscribe(node, m_vertexCostsTopic->getTopicStd(), qos); + m_costsSynchronizer = new message_filters::Cache(m_vertexCostsSubscriber, 1); + m_costsSynchronizer->registerCallback(std::bind(&MeshDisplay::incomingVertexCosts, this, _1)); } void MeshDisplay::updateTopic() @@ -627,33 +671,33 @@ void MeshDisplay::updateMaterialAndTextureServices() } // Check if the service names are valid - std::string error; - if (!ros::names::validate(m_materialServiceName->getStdString(), error) || - !ros::names::validate(m_textureServiceName->getStdString(), error)) + if (rmw_validate_full_topic_name(m_materialServiceName->getStdString().c_str(), nullptr, nullptr) != RMW_TOPIC_VALID || + rmw_validate_full_topic_name(m_textureServiceName->getStdString().c_str(), nullptr, nullptr) != RMW_TOPIC_VALID ) { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } // Update material and texture service clients - ros::NodeHandle n; - m_materialsClient = n.serviceClient(m_materialServiceName->getStdString()); - m_textureClient = n.serviceClient(m_textureServiceName->getStdString()); - if (m_materialsClient.exists()) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + m_materialsClient = node->create_client(m_materialServiceName->getStdString()); + m_textureClient = node->create_client(m_textureServiceName->getStdString()); + + if(m_materialsClient->wait_for_service(std::chrono::seconds(5))) { requestMaterials(m_lastUuid); - if (m_textureClient.exists()) + + if (m_textureClient->wait_for_service(1s)) { - setStatus(rviz::StatusProperty::Ok, "Services", "Material and Texture Service OK"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Services", "Material and Texture Service OK"); } else { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist.")); } - } - else - { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Unable to mesh_msgs::srv::GetMaterials service. Start vision_node first."); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist.")); } } @@ -666,23 +710,24 @@ void MeshDisplay::updateVertexColorService() // Check if the service name is valid std::string error; - if (!ros::names::validate(m_vertexColorServiceName->getStdString(), error)) - { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); + + if (rmw_validate_full_topic_name(m_vertexColorServiceName->getStdString().c_str(), nullptr, nullptr) != RMW_TOPIC_VALID) + { + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The service name contains an invalid character.")); return; } // Update vertex color service client - ros::NodeHandle n; - m_vertexColorClient = n.serviceClient(m_vertexColorServiceName->getStdString()); - if (m_vertexColorClient.exists()) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + m_vertexColorClient = node->create_client(m_vertexColorServiceName->getStdString()); + if(m_vertexColorClient->wait_for_service(std::chrono::seconds(5))) { - setStatus(rviz::StatusProperty::Ok, "Services", "Vertex Color Service OK"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Services", "Vertex Color Service OK"); requestVertexColors(m_lastUuid); } else { - setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist.")); + setStatus(rviz_common::properties::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist.")); } } @@ -696,44 +741,45 @@ void MeshDisplay::initialServiceCall() return; } - ros::NodeHandle n; - m_uuidClient = n.serviceClient("get_uuid"); + auto req_uuids = std::make_shared(); + auto fut_uuids = m_uuidClient->async_send_request(req_uuids); - mesh_msgs::GetUUIDs srv_uuids; - if (m_uuidClient.call(srv_uuids)) + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_uuids) == rclcpp::FutureReturnCode::SUCCESS) { - std::vector uuids = srv_uuids.response.uuids; + auto res_uuids = fut_uuids.get(); + + std::vector uuids = res_uuids->uuids; - if (uuids.size() > 0) + if(uuids.size() > 0) { std::string uuid = uuids[0]; - ROS_INFO_STREAM("Initial data available for UUID=" << uuid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Initial data available for UUID=" << uuid); + + // mesh_msgs::srv::GetGeometry srv_geometry; + auto req_geometry = std::make_shared(); + req_geometry->uuid = uuid; - m_geometryClient = n.serviceClient("get_geometry"); - - mesh_msgs::GetGeometry srv_geometry; - srv_geometry.request.uuid = uuid; - if (m_geometryClient.call(srv_geometry)) - { - ROS_INFO_STREAM("Found geometry for UUID=" << uuid); - mesh_msgs::MeshGeometryStamped::ConstPtr geometry = - boost::make_shared(srv_geometry.response.mesh_geometry_stamped); - processMessage(geometry); - } - else + auto fut_geometry = m_geometryClient->async_send_request(req_geometry); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_geometry) == rclcpp::FutureReturnCode::SUCCESS) { - ROS_INFO_STREAM("Could not load geometry. Waiting for callback to trigger ... "); + auto res_geometry = fut_geometry.get(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Found geometry for UUID=" << uuid); + processMessage(res_geometry->mesh_geometry_stamped); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed to receive mesh_msgs::srv::GetGeometry service response"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not load geometry. Waiting for callback to trigger ... "); } } - } - else - { - ROS_INFO("No initial data available, waiting for callback to trigger ..."); + } else { + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "No initial data available, waiting for callback to trigger ..."); } } -void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg) +void MeshDisplay::processMessage( + const mesh_msgs::msg::MeshGeometryStamped& meshMsg) { if (m_ignoreMsgs) { @@ -743,27 +789,27 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& Ogre::Quaternion orientation; Ogre::Vector3 position; - if (!context_->getFrameManager()->getTransform(meshMsg->header.frame_id, meshMsg->header.stamp, position, + if (!context_->getFrameManager()->getTransform(meshMsg.header.frame_id, meshMsg.header.stamp, position, orientation)) { - ROS_ERROR("Error transforming from frame '%s' to frame '%s'", meshMsg->header.frame_id.c_str(), - qPrintable(rviz::Display::fixed_frame_)); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Error transforming from frame '%s' to frame '%s'", meshMsg.header.frame_id.c_str(), + qPrintable(rviz_common::Display::fixed_frame_)); return; } - if (!m_lastUuid.empty() && meshMsg->uuid.compare(m_lastUuid) != 0) + if (!m_lastUuid.empty() && meshMsg.uuid.compare(m_lastUuid) != 0) { - ROS_WARN("Received geometry with new UUID!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received geometry with new UUID!"); m_costCache.clear(); m_selectVertexCostMap->clearOptions(); m_selectVertexCostMap->addOption("-- None --", 0); } - m_lastUuid = meshMsg->uuid; + m_lastUuid = meshMsg.uuid; // set Geometry std::shared_ptr mesh(std::make_shared()); - for (const geometry_msgs::Point& v : meshMsg->mesh_geometry.vertices) + for (const geometry_msgs::msg::Point& v : meshMsg.mesh_geometry.vertices) { Vertex vertex; vertex.x = v.x; @@ -771,7 +817,7 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& vertex.z = v.z; mesh->vertices.push_back(vertex); } - for (const mesh_msgs::MeshTriangleIndices& f : meshMsg->mesh_geometry.faces) + for (const mesh_msgs::msg::MeshTriangleIndices& f : meshMsg.mesh_geometry.faces) { Face face; face.vertexIndices[0] = f.vertex_indices[0]; @@ -784,34 +830,36 @@ void MeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& // set Normals std::vector normals; - for (const geometry_msgs::Point& n : meshMsg->mesh_geometry.vertex_normals) + for (const geometry_msgs::msg::Point& n : meshMsg.mesh_geometry.vertex_normals) { Normal normal(n.x, n.y, n.z); normals.push_back(normal); } setVertexNormals(normals); - requestVertexColors(meshMsg->uuid); - requestMaterials(meshMsg->uuid); + requestVertexColors(meshMsg.uuid); + requestMaterials(meshMsg.uuid); } -void MeshDisplay::incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg) +void MeshDisplay::incomingGeometry( + const mesh_msgs::msg::MeshGeometryStamped::ConstSharedPtr& meshMsg) { m_messagesReceived++; - setStatus(rviz::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); - processMessage(meshMsg); + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received"); + processMessage(*meshMsg); } -void MeshDisplay::incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr& colorsStamped) +void MeshDisplay::incomingVertexColors( + const mesh_msgs::msg::MeshVertexColorsStamped::ConstSharedPtr& colorsStamped) { if (colorsStamped->uuid.compare(m_lastUuid) != 0) { - ROS_ERROR("Received vertex colors, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received vertex colors, but UUIDs dont match!"); return; } std::vector vertexColors; - for (const std_msgs::ColorRGBA c : colorsStamped->mesh_vertex_colors.vertex_colors) + for (const std_msgs::msg::ColorRGBA c : colorsStamped->mesh_vertex_colors.vertex_colors) { Color color(c.r, c.g, c.b, c.a); vertexColors.push_back(color); @@ -820,11 +868,12 @@ void MeshDisplay::incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped: setVertexColors(vertexColors); } -void MeshDisplay::incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr& costsStamped) +void MeshDisplay::incomingVertexCosts( + const mesh_msgs::msg::MeshVertexCostsStamped::ConstSharedPtr& costsStamped) { if (costsStamped->uuid.compare(m_lastUuid) != 0) { - ROS_ERROR("Received vertex costs, but UUIDs dont match!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received vertex costs, but UUIDs dont match!"); return; } @@ -839,16 +888,19 @@ void MeshDisplay::requestVertexColors(std::string uuid) return; } - mesh_msgs::GetVertexColors srv; - srv.request.uuid = uuid; - if (m_vertexColorClient.call(srv)) + auto req_vertex_colors = std::make_shared(); + req_vertex_colors->uuid = uuid; + + auto fut_vertex_colors = m_vertexColorClient->async_send_request(req_vertex_colors); + + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_vertex_colors) == rclcpp::FutureReturnCode::SUCCESS) { - ROS_INFO("Successful vertex colors service call!"); - mesh_msgs::MeshVertexColorsStamped::ConstPtr meshVertexColors = - boost::make_shared(srv.response.mesh_vertex_colors_stamped); + auto res_vertex_colors = fut_vertex_colors.get(); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successful vertex colors service call!"); std::vector vertexColors; - for (const std_msgs::ColorRGBA c : meshVertexColors->mesh_vertex_colors.vertex_colors) + for (const std_msgs::msg::ColorRGBA c : res_vertex_colors->mesh_vertex_colors_stamped.mesh_vertex_colors.vertex_colors) { Color color(c.r, c.g, c.b, c.a); vertexColors.push_back(color); @@ -858,7 +910,7 @@ void MeshDisplay::requestVertexColors(std::string uuid) } else { - ROS_INFO("Failed vertex colors service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed vertex colors service call!"); } } @@ -869,28 +921,34 @@ void MeshDisplay::requestMaterials(std::string uuid) return; } - mesh_msgs::GetMaterials srv; - srv.request.uuid = uuid; - if (m_materialsClient.call(srv)) + mesh_msgs::srv::GetMaterials srv; + auto req_materials = std::make_shared(); + req_materials->uuid = uuid; + + auto fut_materials = m_materialsClient->async_send_request(req_materials); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_materials) == rclcpp::FutureReturnCode::SUCCESS) { - ROS_INFO("Successful materials service call!"); + auto res_materials = fut_materials.get(); + + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successful materials service call!"); - mesh_msgs::MeshMaterialsStamped::ConstPtr meshMaterialsStamped = - boost::make_shared(srv.response.mesh_materials_stamped); + const mesh_msgs::msg::MeshMaterialsStamped& meshMaterialsStamped = + res_materials->mesh_materials_stamped; - std::vector materials(meshMaterialsStamped->mesh_materials.materials.size()); - for (int i = 0; i < meshMaterialsStamped->mesh_materials.materials.size(); i++) + std::vector materials(meshMaterialsStamped.mesh_materials.materials.size()); + for (int i = 0; i < meshMaterialsStamped.mesh_materials.materials.size(); i++) { - const mesh_msgs::MeshMaterial& mat = meshMaterialsStamped->mesh_materials.materials[i]; + const mesh_msgs::msg::MeshMaterial& mat = meshMaterialsStamped.mesh_materials.materials[i]; materials[i].textureIndex = mat.texture_index; materials[i].color = Color(mat.color.r, mat.color.g, mat.color.b, mat.color.a); } - for (int i = 0; i < meshMaterialsStamped->mesh_materials.clusters.size(); i++) + for (int i = 0; i < meshMaterialsStamped.mesh_materials.clusters.size(); i++) { - const mesh_msgs::MeshFaceCluster& clu = meshMaterialsStamped->mesh_materials.clusters[i]; + const mesh_msgs::msg::MeshFaceCluster& clu = meshMaterialsStamped.mesh_materials.clusters[i]; - uint32_t materialIndex = meshMaterialsStamped->mesh_materials.cluster_materials[i]; - const mesh_msgs::MeshMaterial& mat = meshMaterialsStamped->mesh_materials.materials[materialIndex]; + uint32_t materialIndex = meshMaterialsStamped.mesh_materials.cluster_materials[i]; + const mesh_msgs::msg::MeshMaterial& mat = meshMaterialsStamped.mesh_materials.materials[materialIndex]; for (uint32_t face_index : clu.face_indices) { @@ -899,62 +957,68 @@ void MeshDisplay::requestMaterials(std::string uuid) } std::vector textureCoords; - for (const mesh_msgs::MeshVertexTexCoords& coord : meshMaterialsStamped->mesh_materials.vertex_tex_coords) + for (const mesh_msgs::msg::MeshVertexTexCoords& coord : meshMaterialsStamped.mesh_materials.vertex_tex_coords) { textureCoords.push_back(TexCoords(coord.u, coord.v)); } setMaterials(materials, textureCoords); - for (mesh_msgs::MeshMaterial material : meshMaterialsStamped->mesh_materials.materials) + for (mesh_msgs::msg::MeshMaterial material : meshMaterialsStamped.mesh_materials.materials) { if (material.has_texture) { - mesh_msgs::GetTexture texSrv; - texSrv.request.uuid = uuid; - texSrv.request.texture_index = material.texture_index; - if (m_textureClient.call(texSrv)) + auto req_texture = std::make_shared(); + + req_texture->uuid = uuid; + req_texture->texture_index = material.texture_index; + + auto fut_texture = m_textureClient->async_send_request(req_texture); + + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + if(rclcpp::spin_until_future_complete(node, fut_texture) == rclcpp::FutureReturnCode::SUCCESS) { - ROS_INFO("Successful texture service call with index %d!", material.texture_index); - mesh_msgs::MeshTexture::ConstPtr textureMsg = - boost::make_shared(texSrv.response.texture); + auto res_texture = fut_texture.get(); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Successful texture service call with index %d!", material.texture_index); + const mesh_msgs::msg::MeshTexture& textureMsg = + res_texture->texture; Texture texture; - texture.width = textureMsg->image.width; - texture.height = textureMsg->image.height; - texture.channels = textureMsg->image.step; - texture.pixelFormat = textureMsg->image.encoding; - texture.data = textureMsg->image.data; + texture.width = textureMsg.image.width; + texture.height = textureMsg.image.height; + texture.channels = textureMsg.image.step; + texture.pixelFormat = textureMsg.image.encoding; + texture.data = textureMsg.image.data; - addTexture(texture, textureMsg->texture_index); + addTexture(texture, textureMsg.texture_index); } else { - ROS_INFO("Failed texture service call with index %d!", material.texture_index); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed texture service call with index %d!", material.texture_index); } } } } else { - ROS_INFO("Failed materials service call!"); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Failed materials service call!"); } } void MeshDisplay::cacheVertexCosts(std::string layer, const std::vector& costs) { - ROS_INFO_STREAM("Cache vertex cost map '" << layer << "' for UUID "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Cache vertex cost map '" << layer << "' for UUID "); // insert into cache auto it = m_costCache.find(layer); if (it != m_costCache.end()) { - ROS_INFO_STREAM("The cost layer \"" << layer << "\" has been updated."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "The cost layer \"" << layer << "\" has been updated."); m_costCache.erase(layer); } else { - ROS_INFO_STREAM("The cost layer \"" << layer << "\" has been added."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "The cost layer \"" << layer << "\" has been added."); m_selectVertexCostMap->addOptionStd(layer, m_selectVertexCostMap->numChildren()); } @@ -989,7 +1053,7 @@ std::shared_ptr MeshDisplay::addNewVisual() return m_visuals.back(); } -} // End namespace rviz_map_plugin +} // End namespace rviz_mesh_tools_plugins -#include -PLUGINLIB_EXPORT_CLASS(rviz_map_plugin::MeshDisplay, rviz::Display) +#include +PLUGINLIB_EXPORT_CLASS(rviz_mesh_tools_plugins::MeshDisplay, rviz_common::Display) \ No newline at end of file diff --git a/rviz_map_plugin/src/MeshGoalTool.cpp b/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp similarity index 74% rename from rviz_map_plugin/src/MeshGoalTool.cpp rename to rviz_mesh_tools_plugins/src/MeshGoalTool.cpp index 8d7990c..d77fc66 100644 --- a/rviz_map_plugin/src/MeshGoalTool.cpp +++ b/rviz_mesh_tools_plugins/src/MeshGoalTool.cpp @@ -43,39 +43,45 @@ */ -#include "MeshGoalTool.hpp" +#include "rviz_mesh_tools_plugins/MeshGoalTool.hpp" -#include -PLUGINLIB_EXPORT_CLASS( rviz_map_plugin::MeshGoalTool, rviz::Tool ) +#include +PLUGINLIB_EXPORT_CLASS( rviz_mesh_tools_plugins::MeshGoalTool, rviz_common::Tool ) + +namespace rviz_mesh_tools_plugins +{ -namespace rviz_map_plugin{ MeshGoalTool::MeshGoalTool() { shortcut_key_ = 'm'; - topic_property_ = new rviz::StringProperty( "Topic", "goal", + topic_property_ = new rviz_common::properties::StringProperty( "Topic", "goal", "The topic on which to publish the mesh navigation goals.", getPropertyContainer(), SLOT(updateTopic()), this); - switch_bottom_top_ = new rviz::BoolProperty("Switch Bottom/Top", + switch_bottom_top_ = new rviz_common::properties::BoolProperty("Switch Bottom/Top", false, "Enable to stwich the bottom and top.", getPropertyContainer()); } - void MeshGoalTool::onInitialize() - { - MeshPoseTool::onInitialize(); - setName( "Mesh Goal" ); - updateTopic(); - } +void MeshGoalTool::onInitialize() +{ + MeshPoseTool::onInitialize(); + setName( "Mesh Goal" ); + updateTopic(); +} - void MeshGoalTool::updateTopic() - { - pose_pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); - } +void MeshGoalTool::updateTopic() +{ + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = node->create_publisher(topic_property_->getStdString(), 1); +} - void MeshGoalTool::onPoseSet( const Ogre::Vector3& position, const Ogre::Quaternion& orientation ){ - geometry_msgs::PoseStamped msg; +void MeshGoalTool::onPoseSet( + const Ogre::Vector3& position, + const Ogre::Quaternion& orientation ) +{ + geometry_msgs::msg::PoseStamped msg; msg.pose.position.x = position.x; msg.pose.position.y = position.y; msg.pose.position.z = position.z; @@ -106,9 +112,10 @@ MeshGoalTool::MeshGoalTool() msg.pose.orientation.z = ros_orientation.z; msg.pose.orientation.w = ros_orientation.w; - msg.header.stamp = ros::Time::now(); + auto node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + msg.header.stamp = node->now(); msg.header.frame_id = context_->getFixedFrame().toStdString(); - pose_pub_.publish(msg); - } - + pose_pub_->publish(msg); } + +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp new file mode 100644 index 0000000..7b19b8a --- /dev/null +++ b/rviz_mesh_tools_plugins/src/MeshPoseTool.cpp @@ -0,0 +1,224 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * MeshPoseTool.cpp + * + * authors: Sebastian Pütz + * Alexander Mock + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "rviz_rendering/render_window.hpp" + +// #include "rviz_rendering/render_window.hpp" + +#include +#include +#include +#include +#include + +// method is private :( +// #include +#include +#include + +#include + +#include "rviz_mesh_tools_plugins/MeshPoseTool.hpp" + +#include + + +namespace rviz_mesh_tools_plugins +{ + +MeshPoseTool::MeshPoseTool() +:rviz_common::Tool() +,arrow_(NULL) +{ +} + +MeshPoseTool::~MeshPoseTool() +{ + if(arrow_) + { + delete arrow_; + } +} + +void MeshPoseTool::onInitialize() +{ + arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); + arrow_->getSceneNode()->setVisible(false); +} + +void MeshPoseTool::activate() +{ + setStatus("Click and on a mesh_msgs::TriangleMesh to set the position and drag the mouse for the orientation."); + state_ = Position; +} + +void MeshPoseTool::deactivate() +{ + arrow_->getSceneNode()->setVisible(false); +} + +int MeshPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) +{ + int flags = 0; + + if (event.leftDown()) + { + // TODO: check if there is a proper ROS2 equivalent + // RCLCPP_ASSERT(state_ == Position); + assert(state_ == Position); + + Ogre::Ray mouse_ray = getMouseEventRay(event); + + // Find intersection + Ogre::Vector3 int_pos; + Ogre::Vector3 int_normal; + Intersection intersection; + if(selectFace(context_, mouse_ray, intersection)) + { + pos_start_ = intersection.point; + normal_start_ = intersection.normal; + + // Flip normal to camera + Ogre::Vector3 cam_pos = event.panel->getViewController()->getCamera()->getRealPosition(); + Ogre::Vector3 dir_to_cam = cam_pos - pos_start_; + dir_to_cam.normalise(); + // there should be a function doing the dot product + float scalar = dir_to_cam.x * normal_start_.x + + dir_to_cam.y * normal_start_.y + + dir_to_cam.z * normal_start_.z; + + if(scalar < 0) + { + normal_start_ = -normal_start_; + } + + arrow_->setPosition(pos_start_); + state_ = Orientation; + } + } + else if (event.type == QEvent::MouseMove && event.left()) + { + if (state_ == Orientation) + { + Ogre::Plane plane(normal_start_, pos_start_); + Ogre::Ray mouse_ray = getMouseEventRay(event); + + // intersect with plane + auto res = mouse_ray.intersects(plane); + if(res.first) + { + // plane hit + pos_last_ = mouse_ray.getPoint(res.second); + if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) + { + // valid distance + + // right hand coordinate system + // x to the right, y to the top, and -z into the scene + // arrow foreward negative z + Ogre::Vector3 z_axis = -(pos_last_ - pos_start_).normalisedCopy(); + Ogre::Vector3 y_axis = normal_start_; + Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis).normalisedCopy(); + + arrow_->getSceneNode()->setVisible(true); + arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); + + flags |= Render; + } + } + + } + } + else if (event.leftUp()) + { + if (state_ == Orientation) + { + Ogre::Plane plane(normal_start_, pos_start_); + Ogre::Ray mouse_ray = getMouseEventRay(event); + + // intersect with plane + auto res = mouse_ray.intersects(plane); + if(res.first) + { + // plane hit + pos_last_ = mouse_ray.getPoint(res.second); + if( (pos_last_ - pos_start_).squaredLength() > 0.001 * 0.001 ) + { + // valid distance + + // right hand coordinate system + // x to the right, y to the top, and -z into the scene + // arrow foreward negative z + Ogre::Vector3 z_axis = -(pos_last_ - pos_start_).normalisedCopy(); + Ogre::Vector3 y_axis = normal_start_; + Ogre::Vector3 x_axis = y_axis.crossProduct(z_axis).normalisedCopy(); + + arrow_->getSceneNode()->setVisible(true); + arrow_->setOrientation(Ogre::Quaternion(x_axis, y_axis, z_axis)); + + onPoseSet(pos_start_, Ogre::Quaternion(x_axis, y_axis, z_axis)); + + flags |= (Finished | Render); + state_ = Position; + } + } + } + } + + return flags; +} + +} // namespace rviz_mesh_tools_plugins diff --git a/rviz_map_plugin/src/MeshVisual.cpp b/rviz_mesh_tools_plugins/src/MeshVisual.cpp similarity index 82% rename from rviz_map_plugin/src/MeshVisual.cpp rename to rviz_mesh_tools_plugins/src/MeshVisual.cpp index 4dd59b9..0642858 100644 --- a/rviz_map_plugin/src/MeshVisual.cpp +++ b/rviz_mesh_tools_plugins/src/MeshVisual.cpp @@ -50,18 +50,21 @@ * Jan Philipp Vogtherr */ -#include "MeshVisual.hpp" +#include "rviz_mesh_tools_plugins/MeshVisual.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -namespace rviz_map_plugin +#include "rclcpp/rclcpp.hpp" + +namespace rviz_mesh_tools_plugins { Ogre::ColourValue getRainbowColor1(float value) { @@ -93,7 +96,7 @@ Ogre::ColourValue getRainbowColor1(float value) return Ogre::ColourValue(r, g, b, 1.0f); } -MeshVisual::MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID) +MeshVisual::MeshVisual(rviz_common::DisplayContext* context, size_t displayID, size_t meshID, size_t randomID) : m_displayContext(context) , m_prefix(displayID) , m_postfix(meshID) @@ -104,7 +107,7 @@ MeshVisual::MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t m , m_texture_coords_enabled(false) , m_normalsScalingFactor(1) { - ROS_INFO("Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Creating MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); // get or create the scene node Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager(); @@ -115,12 +118,12 @@ MeshVisual::MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t m std::string sceneId = strstream.str(); if (sceneManager->hasSceneNode(sceneId)) { - // ROS_INFO("Attaching to scene: %s", sceneId); + // RCLCPP_INFO("Attaching to scene: %s", sceneId); m_sceneNode = (Ogre::SceneNode*)(rootNode->getChild(sceneId)); } else { - // ROS_INFO("Creating new scene: %s", sceneId); + // RCLCPP_INFO("Creating new scene: %s", sceneId); m_sceneNode = rootNode->createChildSceneNode(sceneId); } @@ -158,7 +161,7 @@ MeshVisual::MeshVisual(rviz::DisplayContext* context, size_t displayID, size_t m MeshVisual::~MeshVisual() { - ROS_INFO("Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Destroying MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); reset(); @@ -189,47 +192,91 @@ MeshVisual::~MeshVisual() void MeshVisual::reset() { - ROS_INFO("Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Resetting MeshVisual %lu_TexturedMesh_%lu_%lu", m_prefix, m_postfix, m_random); std::stringstream sstm; sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "GeneralMaterial_"; - Ogre::MaterialManager::getSingleton().unload(sstm.str()); - Ogre::MaterialManager::getSingleton().remove(sstm.str()); + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping."); + + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); + } + + } + sstm.str(""); sstm.clear(); if (m_vertex_colors_enabled) { sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "Material_" << 1; - Ogre::MaterialManager::getSingleton().unload(sstm.str()); - Ogre::MaterialManager::getSingleton().remove(sstm.str()); + + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping"); + + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); + } + } + sstm.str(""); sstm.clear(); } sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "NormalMaterial"; - Ogre::MaterialManager::getSingleton().unload(sstm.str()); - Ogre::MaterialManager::getSingleton().remove(sstm.str()); + if(auto materialptr = Ogre::MaterialManager::getSingleton().getByName(sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME)) + { + materialptr->unload(); + Ogre::MaterialManager::getSingleton().remove(materialptr); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Could not find material '" << sstm.str() << "' to unload. skipping"); + + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Available materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); + } + } + sstm.str(""); sstm.clear(); for (Ogre::MaterialPtr textureMaterial : m_textureMaterials) { - Ogre::MaterialManager::getSingleton().unload(textureMaterial->getName()); - Ogre::MaterialManager::getSingleton().remove(textureMaterial->getName()); + textureMaterial->unload(); + Ogre::MaterialManager::getSingleton().remove(textureMaterial); } if (!m_noTexCluMaterial.isNull()) { - Ogre::MaterialManager::getSingleton().unload(m_noTexCluMaterial->getName()); - Ogre::MaterialManager::getSingleton().remove(m_noTexCluMaterial->getName()); + m_noTexCluMaterial->unload(); + Ogre::MaterialManager::getSingleton().remove(m_noTexCluMaterial); } if (!m_vertexCostMaterial.isNull()) { - Ogre::MaterialManager::getSingleton().unload(m_vertexCostMaterial->getName()); - Ogre::MaterialManager::getSingleton().remove(m_vertexCostMaterial->getName()); + m_vertexCostMaterial->unload(); + Ogre::MaterialManager::getSingleton().remove(m_vertexCostMaterial); } m_mesh->clear(); @@ -253,6 +300,15 @@ void MeshVisual::reset() m_texture_coords_enabled = false; m_textures_enabled = false; m_vertex_costs_enabled = false; + + + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "MeshVisual reset done. Left materials are:"); + auto mit = Ogre::MaterialManager::getSingleton().getResourceIterator(); + while(mit.hasMoreElements()) + { + Ogre::ResourcePtr material = mit.getNext(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rviz_mesh_tools_plugins"), "- " << material->getName() << ", group: " << material->getGroup()); + } } void MeshVisual::showWireframe(Ogre::Pass* pass, Ogre::ColourValue wireframeColor, float wireframeAlpha) @@ -349,11 +405,12 @@ void MeshVisual::updateMaterial(bool showFaces, Ogre::ColourValue facesColor, fl } } -void MeshVisual::updateMaterial(bool showWireframe, Ogre::ColourValue wireframeColor, float wireframeAlpha, - bool showFaces, Ogre::ColourValue facesColor, float facesAlpha, - bool useVertexColors, bool showVertexCosts, bool showTextures, - bool showTexturedFacesOnly, bool showNormals, Ogre::ColourValue normalsColor, - float normalsAlpha, float normalsScalingFactor) +void MeshVisual::updateMaterial( + bool showWireframe, Ogre::ColourValue wireframeColor, float wireframeAlpha, + bool showFaces, Ogre::ColourValue facesColor, float facesAlpha, + bool useVertexColors, bool showVertexCosts, bool showTextures, + bool showTexturedFacesOnly, bool showNormals, Ogre::ColourValue normalsColor, + float normalsAlpha, float normalsScalingFactor) { // remove all passes if (!m_meshGeneralMaterial.isNull()) @@ -554,7 +611,7 @@ void MeshVisual::enteringTriangleMeshWithVertexCosts(const Geometry& mesh, const float range = maxCost - minCost; if (range <= 0) { - ROS_ERROR("Illegal vertex cost limits!"); + RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Illegal vertex cost limits!"); return; } @@ -651,7 +708,7 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector // if the image was only default constructed, in which case its width will be 0 if (m_images.size() < textureIndex + 1 || m_images[textureIndex].getWidth() == 0) { - ROS_DEBUG("Texture with index %u not loaded yet", textureIndex); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Texture with index %u not loaded yet", textureIndex); } else { @@ -722,15 +779,16 @@ void MeshVisual::enteringTexturedTriangleMesh(const Geometry& mesh, const vector void MeshVisual::enteringNormals(const Geometry& mesh, const vector& normals) { - if (!m_vertex_normals_enabled) + if(!m_vertex_normals_enabled) { return; } std::stringstream sstm; - if (m_normalMaterial.isNull()) + if(m_normalMaterial.isNull()) { sstm << m_prefix << "_TexturedMesh_" << m_postfix << "_" << m_random << "NormalMaterial"; + m_normalMaterial = Ogre::MaterialManager::getSingleton().create( sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); m_normalMaterial->getTechnique(0)->removeAllPasses(); @@ -780,7 +838,7 @@ bool MeshVisual::setGeometry(const Geometry& mesh) // check if there are enough vertices given if (mesh.vertices.size() < 3) { - ROS_WARN("Received not enough vertices, can't create mesh!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not enough vertices, can't create mesh!"); return false; } @@ -804,12 +862,12 @@ bool MeshVisual::setNormals(const vector& normals) // check if there are vertex normals for each vertex if (normals.size() == m_geometry.vertices.size()) { - ROS_INFO("Received %lu vertex normals.", normals.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex normals.", normals.size()); m_vertex_normals_enabled = true; } else if (normals.size() > 0) { - ROS_WARN("Received not as much vertex normals as vertices, ignoring vertex normals!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex normals as vertices, ignoring vertex normals!"); return false; } @@ -833,12 +891,12 @@ bool MeshVisual::setVertexColors(const vector& vertexColors) // check if there are vertex colors for each vertex if (vertexColors.size() == m_geometry.vertices.size()) { - ROS_INFO("Received %lu vertex colors.", vertexColors.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex colors.", vertexColors.size()); m_vertex_colors_enabled = true; } else { - ROS_WARN("Received not as much vertex colors as vertices, ignoring the vertex colors!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex colors as vertices, ignoring the vertex colors!"); return false; } @@ -857,19 +915,19 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // // check if these MeshVertexCosts belong to the current mesh and were not already loaded // if (m_meshUuid != vertexCostsMsg->uuid) // { - // ROS_WARN("Can't add vertex costs, uuids do not match."); + // RCLCPP_WARN("Can't add vertex costs, uuids do not match."); // return false; // } // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - ROS_DEBUG("Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - ROS_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -886,19 +944,19 @@ bool MeshVisual::setVertexCosts(const std::vector& vertexCosts, int costC // // check if these MeshVertexCosts belong to the current mesh and were not already loaded // if (m_meshUuid != vertexCostsMsg->uuid) // { - // ROS_WARN("Can't add vertex costs, uuids do not match."); + // RCLCPP_WARN("Can't add vertex costs, uuids do not match."); // return false; // } // check if there are vertex costs for each vertex if (vertexCosts.size() == m_geometry.vertices.size()) { - ROS_DEBUG("Received %lu vertex costs.", vertexCosts.size()); + RCLCPP_DEBUG(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu vertex costs.", vertexCosts.size()); m_vertex_costs_enabled = true; } else { - ROS_WARN("Received not as much vertex costs as vertices, ignoring the vertex costs!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much vertex costs as vertices, ignoring the vertex costs!"); return false; } @@ -914,12 +972,12 @@ bool MeshVisual::setMaterials(const vector& materials, const vector= 0) { - ROS_INFO("Received %lu materials.", materials.size()); + RCLCPP_INFO(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received %lu materials.", materials.size()); m_materials_enabled = true; // enable materials } else { - ROS_WARN("Received zero materials, ignoring materials!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received zero materials, ignoring materials!"); return false; } @@ -927,13 +985,13 @@ bool MeshVisual::setMaterials(const vector& materials, const vector 0) { - ROS_WARN("Received not as much texture coords as vertices, ignoring texture coords!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Received not as much texture coords as vertices, ignoring texture coords!"); } enteringTexturedTriangleMesh(m_geometry, materials, texCoords); @@ -962,7 +1020,7 @@ bool MeshVisual::addTexture(Texture& texture, uint32_t textureIndex) } else { - ROS_WARN("Can't load image into texture material, material does not exist!"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Can't load image into texture material, material does not exist!"); return false; } } @@ -978,7 +1036,7 @@ Ogre::PixelFormat MeshVisual::getOgrePixelFormatFromRosString(std::string encodi return Ogre::PF_BYTE_RGB; } - ROS_WARN("Unknown texture encoding! Using Ogre::PF_UNKNOWN"); + RCLCPP_WARN(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Unknown texture encoding! Using Ogre::PF_UNKNOWN"); return Ogre::PF_UNKNOWN; } @@ -1032,4 +1090,4 @@ void MeshVisual::setFrameOrientation(const Ogre::Quaternion& orientation) m_sceneNode->setOrientation(orientation); } -} // end namespace rviz_map_plugin +} // end namespace rviz_mesh_tools_plugins diff --git a/rviz_map_plugin/src/RvizFileProperty.cpp b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp similarity index 74% rename from rviz_map_plugin/src/RvizFileProperty.cpp rename to rviz_mesh_tools_plugins/src/RvizFileProperty.cpp index a9c32b1..2651f9f 100644 --- a/rviz_map_plugin/src/RvizFileProperty.cpp +++ b/rviz_mesh_tools_plugins/src/RvizFileProperty.cpp @@ -45,25 +45,42 @@ * Malte kleine Piening */ -#include "RvizFileProperty.hpp" +#include "rviz_mesh_tools_plugins/RvizFileProperty.hpp" #include -namespace rviz +namespace rviz_common { -FileProperty::FileProperty(const QString& name, const QString& default_value, const QString& description, - Property* parent, const char* changed_slot, QObject* receiver) - : Property(name, default_value, description, parent, changed_slot, receiver) + +namespace properties +{ + +FileProperty::FileProperty( + const QString& name, const QString& default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + : FilePickerProperty( + name, default_value, description, + parent, changed_slot, receiver) { } -QWidget* FileProperty::createEditor(QWidget* parent, const QStyleOptionViewItem&) +QWidget* FileProperty::createEditor( + QWidget* parent, + const QStyleOptionViewItem&) { QFileDialog* editor = new QFileDialog(nullptr); QStringList filenameFilters; - filenameFilters << tr("*.h5"); filenameFilters << tr("*"); + filenameFilters << tr("*.h5"); + filenameFilters << tr("*.ply"); + filenameFilters << tr("*.obj"); + filenameFilters << tr("*.dae"); + filenameFilters << tr("*.stl"); + filenameFilters << tr("*.3d"); + filenameFilters << tr("*.3ds"); + filenameFilters << tr("*.fbx"); + filenameFilters << tr("*.blend"); editor->setNameFilters(filenameFilters); editor->setViewMode(QFileDialog::Detail); @@ -84,4 +101,5 @@ QWidget* FileProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& return nullptr; } -} // namespace rviz +} // namespace properties +} // namespace rviz_common