3D esdf for path planning - Nvblox

Robotics & Edge Computing Robotics - Isaac Isaac ROS
1

Hi

Im using nvblox on a drone in Isaac sim. I have nvblox up and running in a docker enviroment. It makes a nice TSDF reconstruction. I would like to use the ESDF for path planning, but both of the following topics are empty:

  • /nvblox_node/static_esdf_pointcloud
  • /nvblox_node/pessimistic_static_esdf_pointcloud

I have also tried using the service /nvblox_node/get_esdf_and_gradient alone but also with cumotion’s isaac_ros_esdf_visualizer without any results.

Any suggestions?

Best regards
Kasper

2

Hi @kaspermh

Thank you for your post.

I have some questions:

  • Are you using our Docker container?
  • Did you set up theupdate_esdf_rate_hz with a positive value?
  • Did you set publish_esdf_distance_slice to true?

Best,
Raffaello

3

Hi Raffaello,

Im using the docker, as suggested by the instalation-manual.

Both parameters is set correcly. In esdf_mode: “2D” I get an output on the topic’s, but not in “3D” mode.

Looking in the source code, i notice that it only publishes in 2D mode?

void NvbloxNode::processEsdf()
{
  const rclcpp::Time timestamp = get_clock()->now();
  timing::Timer ros_esdf_timer("ros/esdf");
  timing::Rates::tick("ros/update_esdf");

  timing::Timer esdf_integration_timer("ros/esdf/integrate");
  multi_mapper_->updateEsdf();
  if (newest_integrated_depth_time_ > rclcpp::Time(0, 0, RCL_ROS_TIME)) {
    timing::Delays::tick(
      "ros/esdf_integration",
      nvblox::Time(newest_integrated_depth_time_.nanoseconds()),
      nvblox::Time(now().nanoseconds()));
  }
  esdf_integration_timer.Stop();

  if (params_.esdf_mode == EsdfMode::k2D) {
    timing::Timer esdf_output_timer("ros/esdf/slice_output");

    sliceAndPublishEsdf(
      "static", static_mapper_, static_esdf_pointcloud_publisher_,
      static_map_slice_publisher_, static_occupancy_grid_publisher_,
      params_.distance_map_unknown_value_optimistic);

    if (isUsingHumanOrDynamicMapper(params_.mapping_type)) {
      sliceAndPublishEsdf(
        "dynamic", dynamic_mapper_, dynamic_esdf_pointcloud_publisher_,
        dynamic_map_slice_publisher_, dynamic_occupancy_grid_publisher_,
        params_.distance_map_unknown_value_optimistic);
      sliceAndPublishEsdf(
        "combined_dynamic", static_mapper_, combined_esdf_pointcloud_publisher_,
        combined_map_slice_publisher_, combined_occupancy_grid_publisher_,
        params_.distance_map_unknown_value_optimistic, dynamic_mapper_.get());
    }

    if (params_.output_pessimistic_distance_map) {
      sliceAndPublishEsdf(
        "pessimistic_static", static_mapper_,
        pessimistic_static_esdf_pointcloud_publisher_,
        pessimistic_static_map_slice_publisher_, nullptr,
        params_.distance_map_unknown_value_pessimistic);
    }
  }
}