【在实习期的快乐生活】pcd地图转换为2d栅格地图

2024-02-02 05:05| 来源: 网络整理| 查看: 265

需要一个和pcd点云匹配的2d栅格地图，给move_base导航用。

对octomap_mapping进行了魔改： https://github.com/OctoMap/octomap_mapping

在octomap_server_static做了修改

/* * Copyright (c) 2013, A. Hornung, University of Freiburg * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the University of Freiburg nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include //------------------------ #include #include #include //------------------------ #include #include using octomap_msgs::GetOctomap; #define USAGE "\nUSAGE: octomap_server_static \n" \ " mapfile.bt: OctoMap filename to be loaded (.bt: binary tree, .ot: general octree)\n" using namespace std; using namespace octomap; class OctomapServerStatic { public: OctomapServerStatic(const std::string &filename) : m_octree(NULL), m_worldFrameId("/map") { ros::NodeHandle private_nh("~"); private_nh.param("frame_id", m_worldFrameId, m_worldFrameId); // open file: if (filename.length() updateNode(octomap::point3d(p.x, p.y, p.z), true); } // 更新octomap tree->updateInnerOccupancy(); ROS_INFO("done"); m_octree = tree; } if (!m_octree) { ROS_ERROR("Could not read right octree class in file"); exit(1); } ROS_INFO("Read octree type \"%s\" from file %s", m_octree->getTreeType().c_str(), filename.c_str()); ROS_INFO("Octree resultion: %f, size: %zu", m_octree->getResolution(), m_octree->size()); m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServerStatic::octomapBinarySrv, this); m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServerStatic::octomapFullSrv, this); } ~OctomapServerStatic() { } bool octomapBinarySrv(GetOctomap::Request &req, GetOctomap::Response &res) { ROS_INFO("Sending binary map data on service request"); res.map.header.frame_id = m_worldFrameId; res.map.header.stamp = ros::Time::now(); if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) return false; return true; } bool octomapFullSrv(GetOctomap::Request &req, GetOctomap::Response &res) { ROS_INFO("Sending full map data on service request"); res.map.header.frame_id = m_worldFrameId; res.map.header.stamp = ros::Time::now(); if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) return false; return true; } private: ros::ServiceServer m_octomapBinaryService, m_octomapFullService; ros::NodeHandle m_nh; std::string m_worldFrameId; AbstractOccupancyOcTree *m_octree; }; int main(int argc, char **argv) { ros::init(argc, argv, "octomap_server_static"); std::string mapFilename(""); if (argc == 2) mapFilename = std::string(argv[1]); else { ROS_ERROR("%s", USAGE); exit(1); } try { OctomapServerStatic ms(mapFilename); ros::spin(); } catch (std::runtime_error &e) { ROS_ERROR("octomap_server_static exception: %s", e.what()); exit(2); } exit(0); }

配套的launch文件

yaml文件，放在octomap_server/cfg里面

【本文地址】

公司简介

联系我们