Mismatch between the point cloud slice and distance map slice

[Veng97]

Hello!

I'm using Rviz2 to visualize the distance map slice and I sometimes see a disagreement between the origin of the distance map slice and the point cloud slice. It seems that the origin of the distance map slice sometimes is off by one voxel in either (or both) x and y directions. When i move the robot around I can see the offset shifting around once in a while.
Note that i convert the nvblox distance map slice to a nav_msgs::msg::OccupancyGrid msg to enable visualization in Rviz, but their msg fields should have a 1-1 correspondence.

I think this could be a potential issue in the implementation of: RosConverter::distanceMapSliceFromLayer(), but I'm not sure.

[alexmillane]

Interesting! It's possible you've found a bug.

Could you upload a video showing the effect so I can try to repro? 😃

[Veng97]

I've written a simple example that shows how I convert between a standard nav_msg OccupancyGrid and the nvblox DistanceMapSlice so you can test it quickly.

// CostMap color palattes: http://docs.ros.org/en/jade/api/rviz/html/c++/map__display_8cpp_source.html
constexpr int8_t kNoInformation = 0;
constexpr int8_t kFreeSpace = 1;
constexpr int8_t kMinCost = 2;
constexpr int8_t kMaxCost = 98;
constexpr int8_t kInscribedInflatedObstacle = 99;
constexpr int8_t kLethalObstacle = 100;

ObstacleMapping::ObstacleMapping() : Node("obstacle_mapping") {
  RCLCPP_INFO(this->get_logger(), "Launching Obstacle Mapping Node!");

  occupancy_inflation_distance_ = declare_parameter<float>("occupancy_inflation_distance_m", occupancy_inflation_distance_);
  occupancy_max_distance_ = declare_parameter<float>("occupancy_max_distance_m", occupancy_max_distance_);

  // Publishers
  pub_obstacle_map_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("~/occupancy_grid", rclcpp::SensorDataQoS());

  // Subscribers
  sub_distance_map_slice_ = this->create_subscription<nvblox_msgs::msg::DistanceMapSlice>("/nvblox_node/map_slice", rclcpp::SensorDataQoS(),
                                                                                          std::bind(&ObstacleMapping::DistanceMapSliceCallback, this, std::placeholders::_1));
}

void ObstacleMapping::DistanceMapSliceCallback(const nvblox_msgs::msg::DistanceMapSlice::ConstSharedPtr& msg) {
  // Create occupancy map from nvblox slice
  nav_msgs::msg::OccupancyGrid::UniquePtr occupancy_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>();
  occupancy_grid->header.frame_id = msg->header.frame_id;
  occupancy_grid->header.stamp = msg->header.stamp;

  // Copy map dimensions
  occupancy_grid->info.origin.position = msg->origin;
  occupancy_grid->info.resolution = msg->resolution;
  occupancy_grid->info.width = msg->width;
  occupancy_grid->info.height = msg->height;

  // Allocate the occupancy grid
  size_t map_size = msg->width * msg->height;
  occupancy_grid->data.resize(map_size, kNoInformation);

  // Remap the floating point distance to occupancy grid colors
  const auto k_unknown_value = msg->unknown_value;
  for (size_t i = 0; i < map_size; ++i) {
    // Default: No information
    if (msg->data[i] == k_unknown_value) {
      continue;
    }

    // Lethal obstacle
    if (msg->data[i] <= 0.0F) {
      occupancy_grid->data[i] = kLethalObstacle;
      continue;
    }

    // Inflated obstacle
    if (msg->data[i] <= occupancy_inflation_distance_) {
      occupancy_grid->data[i] = kInscribedInflatedObstacle;
      continue;
    }

    // Free space
    if (msg->data[i] > occupancy_max_distance_) {
      occupancy_grid->data[i] = kFreeSpace;
      continue;
    }

    // Distance scale
    occupancy_grid->data[i] = kMinCost + static_cast<int8_t>((kMaxCost - kMinCost) * (1.0F - std::min((msg->data[i] - occupancy_inflation_distance_) / occupancy_max_distance_, 1.0F)));
  }

  pub_obstacle_map_->publish(std::move(occupancy_grid));
}

The video below should highlight the issue. The point cloud distance slice is shown with the point tiles and the underlying distance map is the OccupancyGrid msg slice. The point cloud distance slice seems completely static, but the other map jumps a bit around. The maps are also not completely aligned, which is likely also related to this same issue.

example(1).webm

[jaiveersinghNV]

Could you please try again with the latest Isaac ROS DP3 Release?

[Veng97]

The jump is no longer present in the new release :)

[alexmillane]

@Veng97 Thank you for your help in finding this. much appreciated :+1: