bug fixed in ocs2_raisim_ros/RaisimHeightmapRosConverter.cpp

[Renkunzhao]

Describe the bug
In this cpp file, when converting between raisim::HeightMap and gridMap, the XSamples and YSamples are reversed. The corrected code is as follows:

grid_map_msgs::GridMapPtr RaisimHeightmapRosConverter::convertHeightmapToGridmap(const raisim::HeightMap& heightMap,
                                                                                 const std::string& frameId) {
  grid_map_msgs::GridMapPtr gridMapMsg(new grid_map_msgs::GridMap());

  gridMapMsg->info.header.frame_id = frameId;
  gridMapMsg->info.header.stamp = ros::Time::now();

  const auto xResolution = heightMap.getXSize() / static_cast<double>(heightMap.getXSamples());
  const auto yResolution = heightMap.getYSize() / static_cast<double>(heightMap.getYSamples());
  if (std::abs(xResolution - yResolution) > 1e-9) {
    throw std::runtime_error("RaisimHeightmapRosConverter::convertHeightmapToGridmap - Resolution in x and y must be identical");
  }
  gridMapMsg->info.resolution = xResolution;

  gridMapMsg->info.length_x = heightMap.getXSize();
  gridMapMsg->info.length_y = heightMap.getYSize();

  gridMapMsg->info.pose.position.x = heightMap.getCenterX();
  gridMapMsg->info.pose.position.y = heightMap.getCenterY();
  gridMapMsg->info.pose.orientation.w = 1.0;

  gridMapMsg->layers.emplace_back("elevation");
  std_msgs::Float32MultiArray dataArray;
  dataArray.layout.dim.resize(2);
  dataArray.layout.dim[0].label = "column_index";
  dataArray.layout.dim[0].stride = heightMap.getHeightVector().size();
  dataArray.layout.dim[0].size = heightMap.getYSamples();
  dataArray.layout.dim[1].label = "row_index";
  dataArray.layout.dim[1].stride = heightMap.getXSamples();
  dataArray.layout.dim[1].size = heightMap.getXSamples();
  dataArray.data.insert(dataArray.data.begin(), heightMap.getHeightVector().rbegin(), heightMap.getHeightVector().rend());
  gridMapMsg->data.push_back(dataArray);

  return gridMapMsg;
}

std::unique_ptr<raisim::HeightMap> RaisimHeightmapRosConverter::convertGridmapToHeightmap(const grid_map_msgs::GridMapConstPtr& gridMap) {
  if (gridMap->data[0].layout.dim[0].label != "column_index" or gridMap->data[0].layout.dim[1].label != "row_index") {
    throw std::runtime_error("RaisimHeightmapRosConverter::convertGridmapToHeightmap - Layout of gridMap currently not supported");
  }

  const int xSamples = gridMap->data[0].layout.dim[1].size;
  const int ySamples = gridMap->data[0].layout.dim[0].size;
  const double xSize = gridMap->info.length_x;
  const double ySize = gridMap->info.length_y;
  const double centerX = gridMap->info.pose.position.x;
  const double centerY = gridMap->info.pose.position.y;

  std::vector<double> height(gridMap->data[0].data.rbegin(), gridMap->data[0].data.rend());

  return std::make_unique<raisim::HeightMap>(xSamples, ySamples, xSize, ySize, centerX, centerY, height);
}