rpos::robot_platforms::objects::CompositeMapReader tCmReader;
std::string strErrMsg;
boost::shared_ptr<rpos::robot_platforms::objects::CompositeMap> spCm(tCmReader.loadFile(strErrMsg, filePath));
if (spCm) {
for(auto itr = spCm->maps().begin(); itr != spCm->maps().end(); ++itr){
auto pLayer = *itr;
std::string usage = pLayer->getUsage();
std::string type = pLayer->getType();
std::cout << "Layer Usage : " << usage << std::endl;
//GridMapLayer, 包含map position, resolution, dimension, map data 等数据
if(type == rpos::robot_platforms::objects::GridMapLayer::Type) {
auto pGridMap = boost::dynamic_pointer_cast<rpos::robot_platforms::objects::GridMapLayer>(pLayer);
std::cout << "Map Position : (" << pGridMap->getOrigin().x() << " , " << pGridMap->getOrigin().y() << ")" <<std::endl;
std::cout << "Map Resolution : (" << pGridMap->getResolution().x() << " , " << pGridMap->getResolution().y() << ")" <<std::endl;
std::cout << "Map Dimension: (" << pGridMap->getDimension().x() << " , " << pGridMap->getDimension().y() << ")" <<std::endl;
std::cout << "Map Data:" << std::endl;
for(auto itr = pGridMap->mapData().begin(); itr != pGridMap->mapData().end(); ++itr){
std::cout << (int)*itr << " " ;
}
std::cout << std::endl << std::endl;
}
//LineMapLayer, 包含虚拟轨道, 虚拟墙
else if(type == rpos::robot_platforms::objects::LineMapLayer::Type){
auto pLineMap = boost::dynamic_pointer_cast<rpos::robot_platforms::objects::LineMapLayer>(pLayer);
for(auto itr = pLineMap->lines().begin(); itr != pLineMap->lines().end(); ++itr){
auto line = itr->second;
std::cout << "start: (" << line.start.x() << " , " << line.start.y() << ")" << std::endl;
std::cout << "end: (" << line.end.x() << " , " << line.end.y() << ")" << std::endl;
}
std::cout << std::endl;
//PoseMapLayer, 包含充电桩位置
}else if(type == rpos::robot_platforms::objects::PoseMapLayer::Type){
auto pPoseMap = boost::dynamic_pointer_cast<rpos::robot_platforms::objects::PoseMapLayer>(pLayer);
for(auto itr = pPoseMap->poses().begin(); itr != pPoseMap->poses().end(); ++itr){
auto pos = itr->second;
std::cout << "Position : (" << pos.pose.x() << " , " << pos.pose.y() << ")" << std::endl;
}
std::cout << std::endl;
}else if (type == rpos::robot_platforms::objects::PointsMapLayer::Type){
//get Points map layer
std::cout << std::endl;
}else {
// get unknown map layer
std::cout << std::endl;
}
}
return true;
}
return true; |