Open Renkunzhao opened 5 months ago
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); }
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: