From aadb4fa32237e82b003739be01536f2f8da233c2 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 11:05:13 +0100 Subject: [PATCH 1/8] Starting to implement voxblox map manager. --- nbvplanner/CMakeLists.txt | 7 +-- .../include/nbvplanner/voxblox_map_manager.h | 49 +++++++++++++++++++ nbvplanner/package.xml | 6 ++- nbvplanner/src/voxblox_map_manager.cpp | 29 +++++++++++ 4 files changed, 86 insertions(+), 5 deletions(-) create mode 100644 nbvplanner/include/nbvplanner/voxblox_map_manager.h create mode 100644 nbvplanner/src/voxblox_map_manager.cpp diff --git a/nbvplanner/CMakeLists.txt b/nbvplanner/CMakeLists.txt index 07f7f56b..b9327dc2 100644 --- a/nbvplanner/CMakeLists.txt +++ b/nbvplanner/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS tf kdtree multiagent_collision_check + voxblox_ros ) find_package(cmake_modules REQUIRED) find_package(Eigen REQUIRED) @@ -32,7 +33,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} LIBRARIES nbvplanner ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} - CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs octomap_world tf kdtree + CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs octomap_world tf kdtree voxblox_ros ) include_directories( @@ -42,8 +43,8 @@ include_directories( ${OCTOMAP_INCLUDE_DIRS} ) -add_library(nbvPlannerLib src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp) -add_executable(nbvPlanner src/nbv_planner_node.cpp src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp) +add_library(nbvPlannerLib src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp src/voxblox_map_manager.cpp) +add_executable(nbvPlanner src/nbv_planner_node.cpp src/mesh_structure.cpp src/nbvp.cpp src/rrt.cpp src/tree.cpp src/voxblox_map_manager.cpp) add_dependencies(nbvPlannerLib ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(nbvPlannerLib diff --git a/nbvplanner/include/nbvplanner/voxblox_map_manager.h b/nbvplanner/include/nbvplanner/voxblox_map_manager.h new file mode 100644 index 00000000..2624a242 --- /dev/null +++ b/nbvplanner/include/nbvplanner/voxblox_map_manager.h @@ -0,0 +1,49 @@ +#ifndef NBVPLANNER_VOXBLOX_MAP_MANAGER_H_ +#define NBVPLANNER_VOXBLOX_MAP_MANAGER_H_ + +#include +#include + +namespace nbvInspection { + +// A small class that uses a voxblox TSDF server underneath to emulate the +// functionality of Octomap. +// NOTE: for now uses only *T*SDFs, so doesn't compute the full ESDFs and +// doesn't take advantage of faster collision checking! +class VoxbloxMapManager { + public: + enum VoxelStatus { kUnknown = 0, kOccupied, kFree }; + + VoxbloxMapManager(const ros::NodeHandle& nh, + const ros::NodeHandle& nh_private); + + VoxelStatus getVisibility(const Eigen::Vector3d& view_point, + const Eigen::Vector3d& voxel_to_test, + bool stop_at_unknown_cell) const; + + VoxelStatus getVoxelStatus(const Eigen::Vector3d& position) const; + + // Project an axis-aligned bounding box along a line. + VoxelStatus getLineStatusBoundingBox( + const Eigen::Vector3d& start, const Eigen::Vector3d& end, + const Eigen::Vector3d& bounding_box_size) const; + + // Get the voxel status of an axis-aligned bounding box centered at the given + // position. + VoxelStatus getBoundingBoxStatus( + const Eigen::Vector3d& center, + const Eigen::Vector3d& bounding_box_size) const; + + private: + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + voxblox::TsdfServer tsdf_server_; + + // Cached: + voxblox::Layer* tsdf_layer_; +}; + +} // namespace nbvInspection + +#endif // NBVPLANNER_VOXBLOX_MAP_MANAGER_H_ diff --git a/nbvplanner/package.xml b/nbvplanner/package.xml index 9816dcc1..66372553 100644 --- a/nbvplanner/package.xml +++ b/nbvplanner/package.xml @@ -11,7 +11,7 @@ Andreas Bircher catkin - + roscpp tf message_generation @@ -20,7 +20,8 @@ octomap_world kdtree multiagent_collision_check - + voxblox_ros + roscpp tf message_runtime @@ -29,4 +30,5 @@ octomap_world kdtree multiagent_collision_check + voxblox_ros diff --git a/nbvplanner/src/voxblox_map_manager.cpp b/nbvplanner/src/voxblox_map_manager.cpp new file mode 100644 index 00000000..ae23dd18 --- /dev/null +++ b/nbvplanner/src/voxblox_map_manager.cpp @@ -0,0 +1,29 @@ +#include "nbvplanner/voxblox_map_manager.h" + +namespace nbvInspection { + +VoxbloxMapManager::VoxbloxMapManager(const ros::NodeHandle& nh, + const ros::NodeHandle& nh_private) + : nh_(nh), nh_private_(nh_private), tsdf_server_(nh, nh_private) { + tsdf_layer_ = tsdf_server_.getTsdfMapPtr()->getTsdfLayerPtr(); + CHECK_NOTNULL(tsdf_layer_); +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVoxelStatus( + const Eigen::Vector3d& position) const { + voxblox::TsdfVoxel* voxel = tsdf_layer_->getVoxelPtrByCoordinates( + position.cast()); + + if (voxel == nullptr) { + return VoxbloxMapManager::VoxelStatus::kUnknown; + } + if (voxel->weight < 1e-6) { + return VoxbloxMapManager::VoxelStatus::kUnknown; + } + if (voxel->distance > 0.0) { + return VoxbloxMapManager::VoxelStatus::kFree; + } + return VoxbloxMapManager::VoxelStatus::kOccupied; +} + +} // namespace nbvInspection From 7fb8c04a2a24b967f312cbf8ea39d1cd19a02c03 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 11:25:41 +0100 Subject: [PATCH 2/8] Get voxel status and get visibility now implemented. --- nbvplanner/src/voxblox_map_manager.cpp | 48 +++++++++++++++++++++++--- 1 file changed, 44 insertions(+), 4 deletions(-) diff --git a/nbvplanner/src/voxblox_map_manager.cpp b/nbvplanner/src/voxblox_map_manager.cpp index ae23dd18..a43d4808 100644 --- a/nbvplanner/src/voxblox_map_manager.cpp +++ b/nbvplanner/src/voxblox_map_manager.cpp @@ -15,15 +15,55 @@ VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVoxelStatus( position.cast()); if (voxel == nullptr) { - return VoxbloxMapManager::VoxelStatus::kUnknown; + return VoxelStatus::kUnknown; } if (voxel->weight < 1e-6) { - return VoxbloxMapManager::VoxelStatus::kUnknown; + return VoxelStatus::kUnknown; } if (voxel->distance > 0.0) { - return VoxbloxMapManager::VoxelStatus::kFree; + return VoxelStatus::kFree; } - return VoxbloxMapManager::VoxelStatus::kOccupied; + return VoxelStatus::kOccupied; +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVisibility( + const Eigen::Vector3d& view_point, const Eigen::Vector3d& voxel_to_test, + bool stop_at_unknown_cell) const { + // This involves doing a raycast from view point to voxel to test. + // Let's get the global voxel coordinates of both. + float voxel_size = tsdf_layer_->voxel_size(); + float voxel_size_inv = 1.0 / voxel_size; + + // Cut???? + voxblox::LongIndex start_voxel_idx = + voxblox::getGridIndexFromPoint( + view_point.cast(), voxel_size_inv); + voxblox::LongIndex end_voxel_idx = + voxblox::getGridIndexFromPoint( + voxel_to_test.cast(), voxel_size_inv); + // End cut here. + + const voxblox::Point start_scaled = + view_point.cast() * voxel_size_inv; + const voxblox::Point end_scaled = + voxel_to_test.cast() * voxel_size_inv; + + voxblox::LongIndexVector global_voxel_indices; + voxblox::castRay(start_scaled, end_scaled, &global_voxel_indices); + + // Iterate over the ray. + for (const voxblox::GlobalIndex& global_index : global_voxel_indices) { + voxblox::TsdfVoxel* voxel = + tsdf_layer_->getVoxelPtrByGlobalIndex(global_index); + if (voxel == nullptr || voxel->weight < 1e-6) { + if (stop_at_unknown_cell) { + return VoxelStatus::kUnknown; + } + } else if (voxel->distance <= 0.0) { + return VoxelStatus::kOccupied; + } + } + return VoxelStatus::kFree; } } // namespace nbvInspection From 4fbf610e90974ee1a07da9c99e493edf61ac2259 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 13:11:22 +0100 Subject: [PATCH 3/8] Implemented bounding box check. --- .../include/nbvplanner/voxblox_map_manager.h | 8 ++- nbvplanner/src/voxblox_map_manager.cpp | 62 +++++++++++++++---- 2 files changed, 56 insertions(+), 14 deletions(-) diff --git a/nbvplanner/include/nbvplanner/voxblox_map_manager.h b/nbvplanner/include/nbvplanner/voxblox_map_manager.h index 2624a242..29bde372 100644 --- a/nbvplanner/include/nbvplanner/voxblox_map_manager.h +++ b/nbvplanner/include/nbvplanner/voxblox_map_manager.h @@ -19,20 +19,22 @@ class VoxbloxMapManager { VoxelStatus getVisibility(const Eigen::Vector3d& view_point, const Eigen::Vector3d& voxel_to_test, - bool stop_at_unknown_cell) const; + bool stop_at_unknown_voxel) const; VoxelStatus getVoxelStatus(const Eigen::Vector3d& position) const; // Project an axis-aligned bounding box along a line. VoxelStatus getLineStatusBoundingBox( const Eigen::Vector3d& start, const Eigen::Vector3d& end, - const Eigen::Vector3d& bounding_box_size) const; + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const; // Get the voxel status of an axis-aligned bounding box centered at the given // position. VoxelStatus getBoundingBoxStatus( const Eigen::Vector3d& center, - const Eigen::Vector3d& bounding_box_size) const; + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const; private: ros::NodeHandle nh_; diff --git a/nbvplanner/src/voxblox_map_manager.cpp b/nbvplanner/src/voxblox_map_manager.cpp index a43d4808..5548a9e1 100644 --- a/nbvplanner/src/voxblox_map_manager.cpp +++ b/nbvplanner/src/voxblox_map_manager.cpp @@ -28,21 +28,12 @@ VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVoxelStatus( VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVisibility( const Eigen::Vector3d& view_point, const Eigen::Vector3d& voxel_to_test, - bool stop_at_unknown_cell) const { + bool stop_at_unknown_voxel) const { // This involves doing a raycast from view point to voxel to test. // Let's get the global voxel coordinates of both. float voxel_size = tsdf_layer_->voxel_size(); float voxel_size_inv = 1.0 / voxel_size; - // Cut???? - voxblox::LongIndex start_voxel_idx = - voxblox::getGridIndexFromPoint( - view_point.cast(), voxel_size_inv); - voxblox::LongIndex end_voxel_idx = - voxblox::getGridIndexFromPoint( - voxel_to_test.cast(), voxel_size_inv); - // End cut here. - const voxblox::Point start_scaled = view_point.cast() * voxel_size_inv; const voxblox::Point end_scaled = @@ -56,7 +47,7 @@ VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVisibility( voxblox::TsdfVoxel* voxel = tsdf_layer_->getVoxelPtrByGlobalIndex(global_index); if (voxel == nullptr || voxel->weight < 1e-6) { - if (stop_at_unknown_cell) { + if (stop_at_unknown_voxel) { return VoxelStatus::kUnknown; } } else if (voxel->distance <= 0.0) { @@ -66,4 +57,53 @@ VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getVisibility( return VoxelStatus::kFree; } +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getBoundingBoxStatus( + const Eigen::Vector3d& center, const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const { + float voxel_size = tsdf_layer_->voxel_size(); + float voxel_size_inv = 1.0 / voxel_size; + + // Get the center of the bounding box as a global index. + voxblox::LongIndex center_voxel_index = + voxblox::getGridIndexFromPoint( + center.cast(), voxel_size_inv); + + // Get the bounding box size in terms of voxels. + voxblox::AnyIndex bounding_box_voxels( + std::ceil(bounding_box_size.x() * voxel_size_inv), + std::ceil(bounding_box_size.y() * voxel_size_inv), + std::ceil(bounding_box_size.z() * voxel_size_inv)); + + // Iterate over all voxels in the bounding box. + VoxelStatus current_status = VoxelStatus::kFree; + + voxblox::LongIndex voxel_index = center_voxel_index; + for (voxel_index.x() = center_voxel_index.x() - bounding_box_voxels.x() / 2; + voxel_index.x() <= center_voxel_index.x() + bounding_box_voxels.x() / 2; + voxel_index.x()++) { + for (voxel_index.y() = center_voxel_index.y() - bounding_box_voxels.y() / 2; + voxel_index.y() <= + center_voxel_index.y() + bounding_box_voxels.y() / 2; + voxel_index.y()++) { + for (voxel_index.z() = + center_voxel_index.z() - bounding_box_voxels.z() / 2; + voxel_index.z() <= + center_voxel_index.z() + bounding_box_voxels.z() / 2; + voxel_index.z()++) { + voxblox::TsdfVoxel* voxel = + tsdf_layer_->getVoxelPtrByGlobalIndex(voxel_index); + if (voxel == nullptr || voxel->weight < 1e-6) { + if (stop_at_unknown_voxel) { + return VoxelStatus::kUnknown; + } + current_status = VoxelStatus::kUnknown; + } else if (voxel->distance <= 0.0) { + return VoxelStatus::kOccupied; + } + } + } + } + return current_status; +} + } // namespace nbvInspection From c1afd547340cfcc7a24f4d044854264bec8be8ac Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 13:21:13 +0100 Subject: [PATCH 4/8] All the functions compile now. --- .../include/nbvplanner/voxblox_map_manager.h | 20 +++--- nbvplanner/src/voxblox_map_manager.cpp | 61 ++++++++++++++++--- 2 files changed, 66 insertions(+), 15 deletions(-) diff --git a/nbvplanner/include/nbvplanner/voxblox_map_manager.h b/nbvplanner/include/nbvplanner/voxblox_map_manager.h index 29bde372..d673d898 100644 --- a/nbvplanner/include/nbvplanner/voxblox_map_manager.h +++ b/nbvplanner/include/nbvplanner/voxblox_map_manager.h @@ -24,19 +24,23 @@ class VoxbloxMapManager { VoxelStatus getVoxelStatus(const Eigen::Vector3d& position) const; // Project an axis-aligned bounding box along a line. - VoxelStatus getLineStatusBoundingBox( - const Eigen::Vector3d& start, const Eigen::Vector3d& end, - const Eigen::Vector3d& bounding_box_size, - bool stop_at_unknown_voxel) const; + VoxelStatus getLineStatusBoundingBox(const Eigen::Vector3d& start, + const Eigen::Vector3d& end, + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const; // Get the voxel status of an axis-aligned bounding box centered at the given // position. - VoxelStatus getBoundingBoxStatus( - const Eigen::Vector3d& center, - const Eigen::Vector3d& bounding_box_size, - bool stop_at_unknown_voxel) const; + VoxelStatus getBoundingBoxStatus(const Eigen::Vector3d& center, + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const; private: + VoxelStatus getBoundingBoxStatusInVoxels( + const voxblox::LongIndex& bounding_box_center, + const voxblox::AnyIndex& bounding_box_voxels, + bool stop_at_unknown_voxel) const; + ros::NodeHandle nh_; ros::NodeHandle nh_private_; diff --git a/nbvplanner/src/voxblox_map_manager.cpp b/nbvplanner/src/voxblox_map_manager.cpp index 5548a9e1..097d3b85 100644 --- a/nbvplanner/src/voxblox_map_manager.cpp +++ b/nbvplanner/src/voxblox_map_manager.cpp @@ -75,20 +75,29 @@ VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getBoundingBoxStatus( std::ceil(bounding_box_size.z() * voxel_size_inv)); // Iterate over all voxels in the bounding box. + return getBoundingBoxStatusInVoxels(center_voxel_index, bounding_box_voxels, + stop_at_unknown_voxel); +} + +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getBoundingBoxStatusInVoxels( + const voxblox::LongIndex& bounding_box_center, + const voxblox::AnyIndex& bounding_box_voxels, + bool stop_at_unknown_voxel) const { VoxelStatus current_status = VoxelStatus::kFree; - voxblox::LongIndex voxel_index = center_voxel_index; - for (voxel_index.x() = center_voxel_index.x() - bounding_box_voxels.x() / 2; - voxel_index.x() <= center_voxel_index.x() + bounding_box_voxels.x() / 2; + voxblox::LongIndex voxel_index = bounding_box_center; + for (voxel_index.x() = bounding_box_center.x() - bounding_box_voxels.x() / 2; + voxel_index.x() <= bounding_box_center.x() + bounding_box_voxels.x() / 2; voxel_index.x()++) { - for (voxel_index.y() = center_voxel_index.y() - bounding_box_voxels.y() / 2; + for (voxel_index.y() = + bounding_box_center.y() - bounding_box_voxels.y() / 2; voxel_index.y() <= - center_voxel_index.y() + bounding_box_voxels.y() / 2; + bounding_box_center.y() + bounding_box_voxels.y() / 2; voxel_index.y()++) { for (voxel_index.z() = - center_voxel_index.z() - bounding_box_voxels.z() / 2; + bounding_box_center.z() - bounding_box_voxels.z() / 2; voxel_index.z() <= - center_voxel_index.z() + bounding_box_voxels.z() / 2; + bounding_box_center.z() + bounding_box_voxels.z() / 2; voxel_index.z()++) { voxblox::TsdfVoxel* voxel = tsdf_layer_->getVoxelPtrByGlobalIndex(voxel_index); @@ -106,4 +115,42 @@ VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getBoundingBoxStatus( return current_status; } +VoxbloxMapManager::VoxelStatus VoxbloxMapManager::getLineStatusBoundingBox( + const Eigen::Vector3d& start, const Eigen::Vector3d& end, + const Eigen::Vector3d& bounding_box_size, + bool stop_at_unknown_voxel) const { + // Cast ray along the center to make sure we don't miss anything. + float voxel_size = tsdf_layer_->voxel_size(); + float voxel_size_inv = 1.0 / voxel_size; + + const voxblox::Point start_scaled = + start.cast() * voxel_size_inv; + const voxblox::Point end_scaled = + end.cast() * voxel_size_inv; + + voxblox::LongIndexVector global_voxel_indices; + voxblox::castRay(start_scaled, end_scaled, &global_voxel_indices); + + // Get the bounding box size in terms of voxels. + voxblox::AnyIndex bounding_box_voxels( + std::ceil(bounding_box_size.x() * voxel_size_inv), + std::ceil(bounding_box_size.y() * voxel_size_inv), + std::ceil(bounding_box_size.z() * voxel_size_inv)); + + // Iterate over the ray. + VoxelStatus current_status = VoxelStatus::kFree; + for (const voxblox::GlobalIndex& global_index : global_voxel_indices) { + VoxelStatus box_status = getBoundingBoxStatusInVoxels( + global_index, bounding_box_voxels, stop_at_unknown_voxel); + if (box_status == VoxelStatus::kOccupied) { + return box_status; + } + if (stop_at_unknown_voxel && box_status == VoxelStatus::kUnknown) { + return box_status; + } + current_status = box_status; + } + return current_status; +} + } // namespace nbvInspection From 5be69df2b605a98decc1439cb8e34f5637c1f3e1 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 13:40:58 +0100 Subject: [PATCH 5/8] Fix launch files to work with octomap. --- .../launch/flat_exploration.launch | 4 ++-- .../launch/mav_inspector.launch | 19 +++++++++++-------- interface_nbvp_rotors/worlds/flat.world | 1 + 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/interface_nbvp_rotors/launch/flat_exploration.launch b/interface_nbvp_rotors/launch/flat_exploration.launch index ef7c2441..245273be 100644 --- a/interface_nbvp_rotors/launch/flat_exploration.launch +++ b/interface_nbvp_rotors/launch/flat_exploration.launch @@ -9,9 +9,9 @@ - + - + diff --git a/interface_nbvp_rotors/launch/mav_inspector.launch b/interface_nbvp_rotors/launch/mav_inspector.launch index 39b635a6..768b3dab 100644 --- a/interface_nbvp_rotors/launch/mav_inspector.launch +++ b/interface_nbvp_rotors/launch/mav_inspector.launch @@ -7,7 +7,7 @@ - + @@ -18,34 +18,37 @@ - + - + - + - + + + + - + @@ -62,7 +65,7 @@ - + diff --git a/interface_nbvp_rotors/worlds/flat.world b/interface_nbvp_rotors/worlds/flat.world index 9d3142c3..ffe3621c 100644 --- a/interface_nbvp_rotors/worlds/flat.world +++ b/interface_nbvp_rotors/worlds/flat.world @@ -1,5 +1,6 @@ + 1 0 0 10 0 -0 0 From 54eebaa95f5c3e5442a8d004c4c5f92999a6de55 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 13:59:25 +0100 Subject: [PATCH 6/8] Voxblox-ified version now compiling. --- .../launch/mav_inspector.launch | 11 ++++++++- .../include/nbvplanner/mesh_structure.h | 6 ++--- nbvplanner/include/nbvplanner/nbvp.h | 3 ++- nbvplanner/include/nbvplanner/nbvp.hpp | 6 ++--- nbvplanner/include/nbvplanner/rrt.h | 2 +- nbvplanner/include/nbvplanner/tree.h | 5 ++-- nbvplanner/include/nbvplanner/tree.hpp | 4 ++-- .../include/nbvplanner/voxblox_map_manager.h | 6 +++++ nbvplanner/src/mesh_structure.cpp | 8 +++---- nbvplanner/src/rrt.cpp | 24 +++++++++---------- 10 files changed, 46 insertions(+), 29 deletions(-) diff --git a/interface_nbvp_rotors/launch/mav_inspector.launch b/interface_nbvp_rotors/launch/mav_inspector.launch index 768b3dab..2708b924 100644 --- a/interface_nbvp_rotors/launch/mav_inspector.launch +++ b/interface_nbvp_rotors/launch/mav_inspector.launch @@ -57,13 +57,22 @@ - + + + + + + + + + + diff --git a/nbvplanner/include/nbvplanner/mesh_structure.h b/nbvplanner/include/nbvplanner/mesh_structure.h index cb8f01ac..04cc1c70 100644 --- a/nbvplanner/include/nbvplanner/mesh_structure.h +++ b/nbvplanner/include/nbvplanner/mesh_structure.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include "nbvplanner/voxblox_map_manager.h" namespace mesh { @@ -42,7 +42,7 @@ class StlMesh { resolution_ = resolution; } - static void setOctomapManager(volumetric_mapping::OctomapManager * manager) + static void setVoxbloxManager(nbvInspection::VoxbloxMapManager * manager) { manager_ = manager; } @@ -73,7 +73,7 @@ class StlMesh static std::vector cameraVerticalFoV_; static double maxDist_; static std::vector > camBoundNormals_; - static volumetric_mapping::OctomapManager * manager_; + static nbvInspection::VoxbloxMapManager * manager_; static std::vector peer_vehicles_; }; } diff --git a/nbvplanner/include/nbvplanner/nbvp.h b/nbvplanner/include/nbvplanner/nbvp.h index d4205918..360efe32 100644 --- a/nbvplanner/include/nbvplanner/nbvp.h +++ b/nbvplanner/include/nbvplanner/nbvp.h @@ -29,6 +29,7 @@ #include #include #include +#include #define SQ(x) ((x)*(x)) #define SQRT2 0.70711 @@ -56,7 +57,7 @@ class nbvPlanner Params params_; mesh::StlMesh * mesh_; - volumetric_mapping::OctomapManager * manager_; + nbvInspection::VoxbloxMapManager * manager_; bool ready_; diff --git a/nbvplanner/include/nbvplanner/nbvp.hpp b/nbvplanner/include/nbvplanner/nbvp.hpp index bfcfa078..1b2a904f 100644 --- a/nbvplanner/include/nbvplanner/nbvp.hpp +++ b/nbvplanner/include/nbvplanner/nbvp.hpp @@ -36,7 +36,7 @@ nbvInspection::nbvPlanner::nbvPlanner(const ros::NodeHandle& nh, nh_private_(nh_private) { - manager_ = new volumetric_mapping::OctomapManager(nh_, nh_private_); + manager_ = new nbvInspection::VoxbloxMapManager(nh_, nh_private_); // Set up the topics and services params_.inspectionPath_ = nh_.advertise("inspectionPath", 1000); @@ -100,7 +100,7 @@ nbvInspection::nbvPlanner::nbvPlanner(const ros::NodeHandle& nh, if (stlFile.is_open()) { mesh_ = new mesh::StlMesh(stlFile); mesh_->setResolution(params_.meshResolution_); - mesh_->setOctomapManager(manager_); + mesh_->setVoxbloxManager(manager_); mesh_->setCameraParams(params_.camPitch_, params_.camHorizontal_, params_.camVertical_, params_.gainRange_); } else { @@ -175,7 +175,7 @@ bool nbvInspection::nbvPlanner::plannerCallback(nbvplanner::nbvp_srv:: ROS_ERROR_THROTTLE(1, "Planner not set up: No octomap available!"); return true; } - if (manager_->getMapSize().norm() <= 0.0) { + if (manager_->isEmpty()) { ROS_ERROR_THROTTLE(1, "Planner not set up: Octomap is empty!"); return true; } diff --git a/nbvplanner/include/nbvplanner/rrt.h b/nbvplanner/include/nbvplanner/rrt.h index 7fbf044f..28aeadc7 100644 --- a/nbvplanner/include/nbvplanner/rrt.h +++ b/nbvplanner/include/nbvplanner/rrt.h @@ -38,7 +38,7 @@ class RrtTree : public TreeBase typedef Eigen::Vector4d StateVec; RrtTree(); - RrtTree(mesh::StlMesh * mesh, volumetric_mapping::OctomapManager * manager); + RrtTree(mesh::StlMesh * mesh, VoxbloxMapManager * manager); ~RrtTree(); virtual void setStateFromPoseMsg(const geometry_msgs::PoseWithCovarianceStamped& pose); virtual void setStateFromOdometryMsg(const nav_msgs::Odometry& pose); diff --git a/nbvplanner/include/nbvplanner/tree.h b/nbvplanner/include/nbvplanner/tree.h index 3a855e18..d7dbb53a 100644 --- a/nbvplanner/include/nbvplanner/tree.h +++ b/nbvplanner/include/nbvplanner/tree.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace nbvInspection { @@ -95,14 +96,14 @@ class TreeBase Node * bestNode_; Node * rootNode_; mesh::StlMesh * mesh_; - volumetric_mapping::OctomapManager * manager_; + nbvInspection::VoxbloxMapManager * manager_; stateVec root_; stateVec exact_root_; std::vector*> segments_; std::vector agentNames_; public: TreeBase(); - TreeBase(mesh::StlMesh * mesh, volumetric_mapping::OctomapManager * manager); + TreeBase(mesh::StlMesh * mesh, nbvInspection::VoxbloxMapManager * manager); ~TreeBase(); virtual void setStateFromPoseMsg(const geometry_msgs::PoseWithCovarianceStamped& pose) = 0; virtual void setStateFromOdometryMsg(const nav_msgs::Odometry& pose) = 0; diff --git a/nbvplanner/include/nbvplanner/tree.hpp b/nbvplanner/include/nbvplanner/tree.hpp index a78166de..cb8d5a02 100644 --- a/nbvplanner/include/nbvplanner/tree.hpp +++ b/nbvplanner/include/nbvplanner/tree.hpp @@ -48,7 +48,7 @@ nbvInspection::TreeBase::TreeBase() template nbvInspection::TreeBase::TreeBase(mesh::StlMesh * mesh, - volumetric_mapping::OctomapManager * manager) + nbvInspection::VoxbloxMapManager * manager) { mesh_ = mesh; manager_ = manager; @@ -106,7 +106,7 @@ template void nbvInspection::TreeBase::insertPointcloudWithTf( const sensor_msgs::PointCloud2::ConstPtr& pointcloud) { - manager_->insertPointcloudWithTf(pointcloud); + // manager_->insertPointcloudWithTf(pointcloud); } template diff --git a/nbvplanner/include/nbvplanner/voxblox_map_manager.h b/nbvplanner/include/nbvplanner/voxblox_map_manager.h index d673d898..9376307e 100644 --- a/nbvplanner/include/nbvplanner/voxblox_map_manager.h +++ b/nbvplanner/include/nbvplanner/voxblox_map_manager.h @@ -35,6 +35,12 @@ class VoxbloxMapManager { const Eigen::Vector3d& bounding_box_size, bool stop_at_unknown_voxel) const; + double getResolution() const { return tsdf_layer_->voxel_size(); } + + bool isEmpty() const { + return tsdf_layer_->getNumberOfAllocatedBlocks() == 0; + } + private: VoxelStatus getBoundingBoxStatusInVoxels( const voxblox::LongIndex& bounding_box_center, diff --git a/nbvplanner/src/mesh_structure.cpp b/nbvplanner/src/mesh_structure.cpp index c1d5315d..ebc4c57b 100644 --- a/nbvplanner/src/mesh_structure.cpp +++ b/nbvplanner/src/mesh_structure.cpp @@ -430,7 +430,7 @@ bool mesh::StlMesh::getVisibility(const tf::Transform& transform, bool& partialV || manager_->getVisibility( Eigen::Vector3d(originTransf.x(), originTransf.y(), originTransf.z()), Eigen::Vector3d(x1_.x(), x1_.y(), x1_.z()), stop_at_unknown_cell) - != volumetric_mapping::OctomapWorld::CellStatus::kFree) { + != nbvInspection::VoxbloxMapManager::kFree) { return false; } else { bool visibility1 = true; @@ -454,7 +454,7 @@ bool mesh::StlMesh::getVisibility(const tf::Transform& transform, bool& partialV || manager_->getVisibility( Eigen::Vector3d(originTransf.x(), originTransf.y(), originTransf.z()), Eigen::Vector3d(x2_.x(), x2_.y(), x2_.z()), stop_at_unknown_cell) - != volumetric_mapping::OctomapWorld::CellStatus::kFree) { + != nbvInspection::VoxbloxMapManager::kFree) { ret = false; } else { bool visibility2 = true; @@ -482,7 +482,7 @@ bool mesh::StlMesh::getVisibility(const tf::Transform& transform, bool& partialV || manager_->getVisibility( Eigen::Vector3d(originTransf.x(), originTransf.y(), originTransf.z()), Eigen::Vector3d(x3_.x(), x3_.y(), x3_.z()), stop_at_unknown_cell) - != volumetric_mapping::OctomapWorld::CellStatus::kFree) { + != nbvInspection::VoxbloxMapManager::kFree) { ret = false; } else { bool visibility3 = true; @@ -514,7 +514,7 @@ std::vector mesh::StlMesh::cameraHorizontalFoV_ = { }; std::vector mesh::StlMesh::cameraVerticalFoV_ = { }; double mesh::StlMesh::maxDist_ = 5; std::vector > mesh::StlMesh::camBoundNormals_ = { }; -volumetric_mapping::OctomapManager * mesh::StlMesh::manager_ = NULL; +nbvInspection::VoxbloxMapManager* mesh::StlMesh::manager_ = NULL; std::vector mesh::StlMesh::peer_vehicles_ = { }; #endif // _MESH_STRUCTURE_CPP_ diff --git a/nbvplanner/src/rrt.cpp b/nbvplanner/src/rrt.cpp index f9d65018..d341a70f 100644 --- a/nbvplanner/src/rrt.cpp +++ b/nbvplanner/src/rrt.cpp @@ -51,7 +51,7 @@ nbvInspection::RrtTree::RrtTree() } } -nbvInspection::RrtTree::RrtTree(mesh::StlMesh * mesh, volumetric_mapping::OctomapManager * manager) +nbvInspection::RrtTree::RrtTree(mesh::StlMesh * mesh, nbvInspection::VoxbloxMapManager * manager) { mesh_ = mesh; manager_ = manager; @@ -338,10 +338,10 @@ void nbvInspection::RrtTree::iterate(int iterations) newState[0] = origin[0] + direction[0]; newState[1] = origin[1] + direction[1]; newState[2] = origin[2] + direction[2]; - if (volumetric_mapping::OctomapManager::CellStatus::kFree + if (nbvInspection::VoxbloxMapManager::kFree == manager_->getLineStatusBoundingBox( origin, direction + origin + direction.normalized() * params_.dOvershoot_, - params_.boundingBox_) + params_.boundingBox_, true) && !multiagent::isInCollision(newParent->state_, newState, params_.boundingBox_, segments_)) { // Sample the new orientation newState[3] = 2.0 * M_PI * (((double) rand()) / ((double) RAND_MAX) - 0.5); @@ -434,10 +434,10 @@ void nbvInspection::RrtTree::initialize() newState[0] = origin[0] + direction[0]; newState[1] = origin[1] + direction[1]; newState[2] = origin[2] + direction[2]; - if (volumetric_mapping::OctomapManager::CellStatus::kFree + if (nbvInspection::VoxbloxMapManager::kFree == manager_->getLineStatusBoundingBox( origin, direction + origin + direction.normalized() * params_.dOvershoot_, - params_.boundingBox_) + params_.boundingBox_, true) && !multiagent::isInCollision(newParent->state_, newState, params_.boundingBox_, segments_)) { // Create new node and insert into tree @@ -554,19 +554,19 @@ double nbvInspection::RrtTree::gain(StateVec state) } // Check cell status and add to the gain considering the corresponding factor. double probability; - volumetric_mapping::OctomapManager::CellStatus node = manager_->getCellProbabilityPoint( - vec, &probability); - if (node == volumetric_mapping::OctomapManager::CellStatus::kUnknown) { + nbvInspection::VoxbloxMapManager::VoxelStatus node = manager_->getVoxelStatus( + vec); + if (node == VoxbloxMapManager::kUnknown) { // Rayshooting to evaluate inspectability of cell - if (volumetric_mapping::OctomapManager::CellStatus::kOccupied + if (nbvInspection::VoxbloxMapManager::kOccupied != this->manager_->getVisibility(origin, vec, false)) { gain += params_.igUnmapped_; // TODO: Add probabilistic gain // gain += params_.igProbabilistic_ * PROBABILISTIC_MODEL(probability); } - } else if (node == volumetric_mapping::OctomapManager::CellStatus::kOccupied) { + } else if (node == nbvInspection::VoxbloxMapManager::kOccupied) { // Rayshooting to evaluate inspectability of cell - if (volumetric_mapping::OctomapManager::CellStatus::kOccupied + if (VoxbloxMapManager::kOccupied != this->manager_->getVisibility(origin, vec, false)) { gain += params_.igOccupied_; // TODO: Add probabilistic gain @@ -574,7 +574,7 @@ double nbvInspection::RrtTree::gain(StateVec state) } } else { // Rayshooting to evaluate inspectability of cell - if (volumetric_mapping::OctomapManager::CellStatus::kOccupied + if (VoxbloxMapManager::kOccupied != this->manager_->getVisibility(origin, vec, false)) { gain += params_.igFree_; // TODO: Add probabilistic gain From 25a690122b9f5a3c380dbfe88af191ff7d35abc2 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 27 Nov 2018 14:01:16 +0100 Subject: [PATCH 7/8] Turn off verbosity. --- interface_nbvp_rotors/launch/mav_inspector.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/interface_nbvp_rotors/launch/mav_inspector.launch b/interface_nbvp_rotors/launch/mav_inspector.launch index 2708b924..51786edf 100644 --- a/interface_nbvp_rotors/launch/mav_inspector.launch +++ b/interface_nbvp_rotors/launch/mav_inspector.launch @@ -66,12 +66,14 @@ + + From aed574bae6c74c2f21b75fe975c9b669b9d1061c Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 29 Nov 2018 11:29:25 +0100 Subject: [PATCH 8/8] Switch to normal colors. --- interface_nbvp_rotors/launch/mav_inspector.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/interface_nbvp_rotors/launch/mav_inspector.launch b/interface_nbvp_rotors/launch/mav_inspector.launch index 51786edf..46a49d59 100644 --- a/interface_nbvp_rotors/launch/mav_inspector.launch +++ b/interface_nbvp_rotors/launch/mav_inspector.launch @@ -68,6 +68,7 @@ +