Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #pragma once
00011
00012 #include <grid_map_core/grid_map_core.hpp>
00013
00014
00015 #include <costmap_2d/costmap_2d.h>
00016 #include <costmap_2d/costmap_2d_ros.h>
00017 #include <tf/tf.h>
00018
00019
00020 #include <vector>
00021
00022 namespace grid_map {
00023
00031 class Costmap2DDirectTranslationTable
00032 {
00033 public:
00034 Costmap2DDirectTranslationTable() {}
00035
00036 template<typename DataType>
00037 static void create(std::vector<DataType>& costTranslationTable) {
00038 costTranslationTable.resize(256);
00039 for (unsigned int i = 0; i < costTranslationTable.size(); ++i ) {
00040 costTranslationTable[i] = static_cast<DataType>(i);
00041 }
00042 }
00043 };
00044
00051 class Costmap2DCenturyTranslationTable
00052 {
00053 public:
00054 Costmap2DCenturyTranslationTable() {}
00055
00056 template<typename DataType>
00057 static void create(std::vector<DataType>& costTranslationTable) {
00058 costTranslationTable.resize(256);
00059 costTranslationTable[0] = 0.0;
00060 costTranslationTable[253] = 99.0;
00061 costTranslationTable[254] = 100.0;
00062 costTranslationTable[255] = -1.0;
00063
00064
00065
00066 for (int i = 1; i < 253; i++) {
00067 costTranslationTable[i] = char(1 + (97 * (i - 1)) / 251);
00068 }
00069 }
00070 };
00071
00072 template<typename DataType>
00073 class Costmap2DDefaultTranslationTable : public Costmap2DDirectTranslationTable {};
00074
00075 template<>
00076 class Costmap2DDefaultTranslationTable<float> : public Costmap2DCenturyTranslationTable {};
00077
00078
00079
00080
00081
00090 template<typename MapType, typename TranslationTable=Costmap2DDefaultTranslationTable<typename MapType::DataType>>
00091 class Costmap2DConverter
00092 {
00093 public:
00094
00099 Costmap2DConverter()
00100 {
00101 TranslationTable::create(costTranslationTable_);
00102 }
00103
00104 virtual ~Costmap2DConverter()
00105 {
00106 }
00107
00108 void initializeFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, MapType& outputMap)
00109 {
00110 initializeFromCostmap2D(*(costmap2d.getCostmap()), outputMap);
00111 outputMap.setFrameId(costmap2d.getGlobalFrameID());
00112 }
00113
00114 void initializeFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, MapType& outputMap)
00115 {
00116 const double resolution = costmap2d.getResolution();
00117 Length length(costmap2d.getSizeInCellsX()*resolution, costmap2d.getSizeInCellsY()*resolution);
00118 Position position(costmap2d.getOriginX(), costmap2d.getOriginY());
00119
00120 position += Position(0.5 * length);
00121 outputMap.setGeometry(length, resolution, position);
00122 }
00123
00132 bool addLayerFromCostmap2D(const costmap_2d::Costmap2D& costmap2d,
00133 const std::string& layer,
00134 MapType& outputMap)
00135 {
00136
00137 Size size(costmap2d.getSizeInCellsX(), costmap2d.getSizeInCellsY());
00138 if ((outputMap.getSize() != size).any()) {
00139 errorMessage_ = "Costmap2D and output map have different sizes!";
00140 return false;
00141 }
00142 if (!outputMap.getStartIndex().isZero()) {
00143 errorMessage_ = "Does not support non-zero start indices!";
00144 return false;
00145 }
00146
00147
00148
00149 typename MapType::Matrix data(size(0), size(1));
00150 const size_t nCells = outputMap.getSize().prod();
00151 for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) {
00152 const unsigned char cost = costmap2d.getCharMap()[j];
00153 data(i) = (float) costTranslationTable_[cost];
00154 }
00155
00156 outputMap.add(layer, data);
00157 return true;
00158 }
00159
00168 bool addLayerFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer,
00169 MapType& outputMap)
00170 {
00171 return addLayerFromCostmap2D(*(costmap2d.getCostmap()), layer, outputMap);
00172 }
00173
00189 bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const Length& length,
00190 MapType& outputMap)
00191 {
00192 const double resolution = costmap2d.getCostmap()->getResolution();
00193
00194
00195 tf::Stamped<tf::Pose> tfPose;
00196 if(!costmap2d.getRobotPose(tfPose))
00197 {
00198 errorMessage_ = "Could not get robot pose, is it actually published?";
00199 return false;
00200 }
00201
00202
00203 Position robotPosition(tfPose.getOrigin().x() , tfPose.getOrigin().y());
00204 Position rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY());
00205 Position newCostMapOrigin;
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216 Position robotCellPosition = (robotPosition - rosMapOrigin) / resolution;
00217
00218
00219
00220
00221
00222
00223 int numberOfCellsX = length.x()/resolution;
00224 int numberOfCellsY = length.y()/resolution;
00225 if (numberOfCellsX % 2) {
00226 newCostMapOrigin(0) = std::floor(robotCellPosition.x())*resolution + resolution/2.0 + rosMapOrigin.x();
00227 } else {
00228 newCostMapOrigin(0) = std::round(robotCellPosition.x())*resolution + rosMapOrigin.x();
00229 }
00230 if (numberOfCellsY % 2) {
00231 newCostMapOrigin(1) = std::floor(robotCellPosition.y())*resolution + resolution/2.0 + rosMapOrigin.y();
00232 } else {
00233 newCostMapOrigin(1) = std::round(robotCellPosition.y())*resolution + rosMapOrigin.y();
00234 }
00235
00236
00237
00238
00239
00240 outputMap.setFrameId(costmap2d.getGlobalFrameID());
00241 outputMap.setTimestamp(ros::Time::now().toNSec());
00242 outputMap.setGeometry(length, resolution, newCostMapOrigin);
00243 return true;
00244 }
00245
00255 bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d,
00256 const std::string& layer, MapType& outputMap)
00257 {
00258
00259
00260
00261 if (outputMap.getResolution() != costmap2d.getCostmap()->getResolution()) {
00262 errorMessage_ = "Costmap2D and output map have different resolutions!";
00263 return false;
00264 }
00265
00266
00267
00268
00269
00270
00271
00272 const double resolution = costmap2d.getCostmap()->getResolution();
00273 const Length geometry = outputMap.getLength();
00274 const Position position = outputMap.getPosition();
00275
00276
00277 bool isValidWindow = false;
00278 costmap_2d::Costmap2D costmapSubwindow;
00279
00280 isValidWindow = costmapSubwindow.copyCostmapWindow(
00281 *(costmap2d.getCostmap()),
00282 position.x() - geometry.x() / 2.0,
00283 position.y() - geometry.y() / 2.0,
00284 geometry.x(),
00285 geometry.y());
00286 if (!isValidWindow) {
00287
00288 errorMessage_ = "Subwindow landed outside the costmap, aborting.";
00289 return false;
00290 }
00291 addLayerFromCostmap2D(costmapSubwindow, layer, outputMap);
00292 return true;
00293 }
00294
00303 std::string errorMessage() const { return errorMessage_; }
00304
00305 private:
00306 std::vector<typename MapType::DataType> costTranslationTable_;
00307 std::string errorMessage_;
00308 };
00309
00310 }