Using Voxblox for Planning

The planners described in Continuous-Time Trajectory Optimization for Online UAV Replanning, Safe Local Exploration for Replanning in Cluttered Unknown Environments for Micro-Aerial Vehicles, and Sparse 3D Topological Graphs for Micro-Aerial Vehicle Planning will be open-sourced shortly.

In the mean-time, the general idea behind using voxblox for planning is to have two nodes running: one for the mapping, which ingests pointcloud data and produces both a TSDF and an ESDF, and one for planning, which subscribes to the latest ESDF layer over ROS.

The planner should have a voxblox::EsdfServer as a member, and simply remap the esdf_map_out and esdf_map_in topics to match.

A sample launch file is shown below:

<launch>
  <arg name="robot_name" default="my_robot" />
  <arg name="voxel_size" default="0.20" />
  <arg name="voxels_per_side" default="16" />
  <arg name="world_frame" default="odom" />
  <group ns="$(arg robot_name)">

    <node name="voxblox_node" pkg="voxblox_ros" type="esdf_server" output="screen" args="-alsologtostderr" clear_params="true">
      <remap from="pointcloud" to="great_sensor/my_pointcloud"/>
      <remap from="voxblox_node/esdf_map_out" to="esdf_map" />
      <param name="tsdf_voxel_size" value="$(arg voxel_size)" />
      <param name="tsdf_voxels_per_side" value="$(arg voxels_per_side)" />
      <param name="publish_esdf_map" value="true" />
      <param name="publish_pointclouds" value="true" />
      <param name="use_tf_transforms" value="true" />
      <param name="update_mesh_every_n_sec" value="1.0" />
      <param name="clear_sphere_for_planning" value="true" />
      <param name="world_frame" value="$(arg world_frame)" />
    </node>

    <node name="my_voxblox_planner" pkg="voxblox_planner" type="my_voxblox_planner" output="screen" args="-alsologtostderr">
      <remap from="odometry" to="great_estimator/odometry" />
      <remap from="my_voxblox_planner/esdf_map_in" to="esdf_map" />
      <param name="tsdf_voxel_size" value="$(arg voxel_size)" />
      <param name="tsdf_voxels_per_side" value="$(arg voxels_per_side)" />
      <param name="update_mesh_every_n_sec" value="0.0" />
      <param name="world_frame" value="$(arg world_frame)" />
    </node>

  </group>
</launch>

And some scaffolding for writing your own planner using ESDF collision checking:

class YourPlannerVoxblox {
 public:
  YourPlannerVoxblox(const ros::NodeHandle& nh,
                    const ros::NodeHandle& nh_private);
  virtual ~YourPlannerVoxblox() {}
  double getMapDistance(const Eigen::Vector3d& position) const;
 private:
  ros::NodeHandle nh_;
  ros::NodeHandle nh_private_;

  // Map!
  voxblox::EsdfServer voxblox_server_;
};

There’s also a traversability pointcloud you can enable/disable, that if you set the radius to your robot’s collision checking radius, can show you parts of the map the planner thinks are traversable in a pointcloud:

YourPlannerVoxblox::YourPlannerVoxblox(const ros::NodeHandle& nh,
                                     const ros::NodeHandle& nh_private)
    : nh_(nh),
      nh_private_(nh_private),
      voxblox_server_(nh_, nh_private_) {
  // Optionally load a map saved with the save_map service call in voxblox.
  std::string input_filepath;
  nh_private_.param("voxblox_path", input_filepath, input_filepath);
  if (!input_filepath.empty()) {
    if (!voxblox_server_.loadMap(input_filepath)) {
      ROS_ERROR("Couldn't load ESDF map!");
    }
  }
  double robot_radius = 1.0;
  voxblox_server_.setTraversabilityRadius(robot_radius);
  voxblox_server_.publishTraversable();
}

Then to check for collisions you can just compare map distance to your robot radius:

double YourPlannerVoxblox::getMapDistance(
    const Eigen::Vector3d& position) const {
  if (!voxblox_server_.getEsdfMapPtr()) {
    return 0.0;
  }
  double distance = 0.0;
  if (!voxblox_server_.getEsdfMapPtr()->getDistanceAtPosition(position,
                                                              &distance)) {
    return 0.0;
  }
  return distance;
}