[DO NOT MERGE] Example of how to swap the backend from octomap to voxblox

interface_nbvp_rotors/launch/flat_exploration.launch

@@ -9,9 +9,9 @@
     <arg name="paused" value="true"/>
     <arg name="gui" value="true"/>
   </include>
-  
+
   <node pkg="tf" type="static_transform_publisher" name="tf_53" args="0 0 0 0 0 0 world navigation 100" />
-  
+
   <include file="$(find interface_nbvp_rotors)/launch/mav_inspector.launch">
     <arg name="mav_name" value="$(arg mav_name)" />
     <arg name="waypoint_x" value="0.0" />

interface_nbvp_rotors/launch/mav_inspector.launch

@@ -7,7 +7,7 @@
   <arg name="enable_logging" default="false" />
   <arg name="enable_ground_truth" default="true" />
   <arg name="log_file" default="$(arg mav_name)" />
-  <arg name="param_file" default="$(find nbvplanner)/resource/exploration.yaml" />
+  <arg name="param_file" default="$(find interface_nbvp_rotors)/resource/exploration.yaml" />
   <arg name="tf_frame" default="navigation" />
   <arg name="pcl_topic" default="/pointcloudOut" />
   <arg name="stl_file_path" default="" />
@@ -18,34 +18,37 @@
   <arg name="peer_vehicle_pose_topic_1" default="peer_pose_1" />
   <arg name="peer_vehicle_pose_topic_2" default="peer_pose_2" />
   <arg name="peer_vehicle_pose_topic_3" default="peer_pose_3" />
-  
+
   <node pkg="tf" type="static_transform_publisher" name="tf_1$(arg subscript)" args="0 0 0 0 0 0 $(arg mav_name)$(arg subscript)/vi_sensor/base_link fcu$(arg subscript) 1" />
   <node pkg="tf" type="static_transform_publisher" name="tf_2$(arg subscript)" args="0.015 0.055 0.0065 -1.57 0.1 -1.57 fcu$(arg subscript) $(arg mav_name)$(arg subscript)/vi_sensor/camera_depth_optical_center_link 1" />
   <node pkg="tf" type="static_transform_publisher" name="tf_3$(arg subscript)" args="0.015 0.055 0.0065 -1.57 0.1 -1.57 fcu$(arg subscript) $(arg mav_name)$(arg subscript)/vi_sensor/camera_left_link 1" />
   <node pkg="tf" type="static_transform_publisher" name="tf_4$(arg subscript)" args="0.015 -0.055 0.0065 -1.57 0.1 -1.57 fcu$(arg subscript) $(arg mav_name)$(arg subscript)/vi_sensor/camera_right_link 1" />
-  
+
   <group ns="$(arg mav_name)$(arg subscript)">
     <param name="wp_x" type="double" value="$(arg waypoint_x)" />
     <param name="wp_y" type="double" value="$(arg waypoint_y)" />
     <param name="wp_z" type="double" value="$(arg waypoint_z)" />
     <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
       <arg name="mav_name" value="$(arg mav_name)$(arg subscript)" />
-      <arg name="model" value="$(find rotors_description)/urdf/firefly_vi_sensor.gazebo" />
+      <arg name="model" value="$(find rotors_description)/urdf/mav_with_vi_sensor.gazebo" />
       <arg name="enable_logging" value="$(arg enable_logging)" />
       <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
       <arg name="log_file" value="$(arg log_file)"/>
-      <arg name="x" value="$(arg waypoint_x)"/>
+      <!--  <arg name="x" value="$(arg waypoint_x)"/>
       <arg name="y" value="$(arg waypoint_y)"/>
-      <arg name="z" value="0.1"/>
+      <arg name="z" value="0.1"/> -->
       <!--arg name="tf_prefix" default="$(arg mav_name)$(arg subscript)"/-->
     </include>
+
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
     <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
       <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
       <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
       <remap from="odometry" to="odometry_sensor1/odometry" />
     </node>
     <node name="nbvPlanner" pkg="nbvplanner" type="nbvPlanner" output="screen">
-      <param name="tf_frame" type="string" value="$(arg tf_frame)" />
+      <param name="tf_frame" type="string" value="$(arg tf_frame)"/>
       <param name="resolution" type="double" value="$(arg resolution)" />
       <param name="visualize_max_z" type="double" value="$(arg visualize_max_z)" />
       <param name="sensor_max_range" type="double" value="$(arg sensor_max_range)" />
@@ -54,15 +57,27 @@
       <param name="map_publish_frequency" type="double" value="1.0" />
       <remap from="cam0/camera_info" to="/$(arg mav_name)$(arg subscript)/vi_sensor/left/camera_info"/>
       <remap from="cam1/camera_info" to="/$(arg mav_name)$(arg subscript)/vi_sensor/right/camera_info"/>
-      <remap from="pointcloud_throttled" to="$(arg pcl_topic)"/>
+      <!-- <remap from="pointcloud_throttled" to="$(arg pcl_topic)"/> -->
       <remap from="pose" to="/$(arg mav_name)$(arg subscript)/ground_truth/pose_with_covariance"/>
       <remap from="nbvplanner" to="/$(arg mav_name)$(arg subscript)/nbvplanner"/>
       <remap from="peer_pose_1" to="$(arg peer_vehicle_pose_topic_1)"/>
       <remap from="peer_pose_2" to="$(arg peer_vehicle_pose_topic_2)"/>
       <remap from="peer_pose_3" to="$(arg peer_vehicle_pose_topic_3)"/>
       <rosparam command="load" file="$(arg param_file)" />
+
+      <!-- settings for voxblox -->
+      <param name="tsdf_voxel_size" value="$(arg resolution)" />
+      <param name="method" value="fast" />
+      <param name="color_mode" value="normals" />
+      <param name="max_ray_length_m" value="10.0" />
+      <param name="update_mesh_every_n_sec" value="1.0" />
+      <param name="use_tf_transforms" value="true" />
+      <param name="voxel_carving_enabled" value="true" />
+      <param name="min_time_between_msgs_sec" value="0.10" />
+      <param name="verbose" value="false" />
+      <remap from="pointcloud" to="$(arg pcl_topic)"/>
     </node>
-    <node name="exploration" pkg="interface_nbvp_rotors" type="exploration" output="screen" >
+    <node name="exploration" pkg="interface_nbvp_rotors" type="exploration" output="screen">
       <rosparam command="load" file="$(arg param_file)" />
     </node>
   </group>

interface_nbvp_rotors/worlds/flat.world

@@ -1,5 +1,6 @@
 <sdf version='1.4'>
   <world name='default'>
+      <plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>
     <light name='sun' type='directional'>
       <cast_shadows>1</cast_shadows>
       <pose>0 0 10 0 -0 0</pose>

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

nbvplanner/include/nbvplanner/mesh_structure.h

@@ -23,7 +23,7 @@
 #include <ros/ros.h>
 #include <geometry_msgs/Pose.h>
 #include <tf/transform_datatypes.h>
-#include <octomap_world/octomap_manager.h>
+#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<double> cameraVerticalFoV_;
   static double maxDist_;
   static std::vector<std::vector<tf::Vector3> > camBoundNormals_;
-  static volumetric_mapping::OctomapManager * manager_;
+  static nbvInspection::VoxbloxMapManager * manager_;
   static std::vector<tf::Vector3> peer_vehicles_;
 };
 }

nbvplanner/include/nbvplanner/nbvp.h

@@ -29,6 +29,7 @@
 #include <nbvplanner/mesh_structure.h>
 #include <nbvplanner/tree.hpp>
 #include <nbvplanner/rrt.h>
+#include <nbvplanner/voxblox_map_manager.h>
 
 #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_;
 

nbvplanner/include/nbvplanner/nbvp.hpp

@@ -36,7 +36,7 @@ nbvInspection::nbvPlanner<stateVec>::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<visualization_msgs::Marker>("inspectionPath", 1000);
@@ -100,7 +100,7 @@ nbvInspection::nbvPlanner<stateVec>::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<stateVec>::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;
   }

nbvplanner/include/nbvplanner/rrt.h

@@ -38,7 +38,7 @@ class RrtTree : public TreeBase<Eigen::Vector4d>
   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);

nbvplanner/include/nbvplanner/tree.h

@@ -24,6 +24,7 @@
 #include <octomap_world/octomap_manager.h>
 #include <multiagent_collision_check/Segment.h>
 #include <nbvplanner/mesh_structure.h>
+#include <nbvplanner/voxblox_map_manager.h>
 
 namespace nbvInspection {
 
@@ -95,14 +96,14 @@ class TreeBase
   Node<stateVec> * bestNode_;
   Node<stateVec> * rootNode_;
   mesh::StlMesh * mesh_;
-  volumetric_mapping::OctomapManager * manager_;
+  nbvInspection::VoxbloxMapManager * manager_;
   stateVec root_;
   stateVec exact_root_;
   std::vector<std::vector<Eigen::Vector3d>*> segments_;
   std::vector<std::string> 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;

nbvplanner/include/nbvplanner/tree.hpp

@@ -48,7 +48,7 @@ nbvInspection::TreeBase<stateVec>::TreeBase()
 
 template<typename stateVec>
 nbvInspection::TreeBase<stateVec>::TreeBase(mesh::StlMesh * mesh,
-                                            volumetric_mapping::OctomapManager * manager)
+                                            nbvInspection::VoxbloxMapManager * manager)
 {
   mesh_ = mesh;
   manager_ = manager;
@@ -106,7 +106,7 @@ template<typename stateVec>
 void nbvInspection::TreeBase<stateVec>::insertPointcloudWithTf(
     const sensor_msgs::PointCloud2::ConstPtr& pointcloud)
 {
-  manager_->insertPointcloudWithTf(pointcloud);
+ // manager_->insertPointcloudWithTf(pointcloud);
 }
 
 template<typename stateVec>

nbvplanner/include/nbvplanner/voxblox_map_manager.h

@@ -0,0 +1,61 @@
+#ifndef NBVPLANNER_VOXBLOX_MAP_MANAGER_H_
+#define NBVPLANNER_VOXBLOX_MAP_MANAGER_H_
+
+#include <ros/ros.h>
+#include <voxblox_ros/tsdf_server.h>
+
+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_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,
+                                       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;
+
+  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,
+      const voxblox::AnyIndex& bounding_box_voxels,
+      bool stop_at_unknown_voxel) const;
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  voxblox::TsdfServer tsdf_server_;
+
+  // Cached:
+  voxblox::Layer<voxblox::TsdfVoxel>* tsdf_layer_;
+};
+
+}  // namespace nbvInspection
+
+#endif  // NBVPLANNER_VOXBLOX_MAP_MANAGER_H_

nbvplanner/package.xml

@@ -11,7 +11,7 @@
   <author>Andreas Bircher</author>
 
   <buildtool_depend>catkin</buildtool_depend>
-  
+
   <build_depend>roscpp</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>message_generation</build_depend>
@@ -20,7 +20,8 @@
   <build_depend>octomap_world</build_depend>
   <build_depend>kdtree</build_depend>
   <build_depend>multiagent_collision_check</build_depend>
-
+  <build_depend>voxblox_ros</build_depend>
+
   <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>message_runtime</run_depend>
@@ -29,4 +30,5 @@
   <run_depend>octomap_world</run_depend>
   <run_depend>kdtree</run_depend>
   <run_depend>multiagent_collision_check</run_depend>
+  <run_depend>voxblox_ros</run_depend>
 </package>

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<double> mesh::StlMesh::cameraHorizontalFoV_ = { };
 std::vector<double> mesh::StlMesh::cameraVerticalFoV_ = { };
 double mesh::StlMesh::maxDist_ = 5;
 std::vector<std::vector<tf::Vector3> > mesh::StlMesh::camBoundNormals_ = { };
-volumetric_mapping::OctomapManager * mesh::StlMesh::manager_ = NULL;
+nbvInspection::VoxbloxMapManager* mesh::StlMesh::manager_ = NULL;
 std::vector<tf::Vector3> mesh::StlMesh::peer_vehicles_ = { };
 
 #endif // _MESH_STRUCTURE_CPP_