Mismatch between the point cloud slice and distance map slice about isaac_ros_nvblox HOT 5 CLOSED

// 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));
}