octomap_server_static.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, A. Hornung, University of Freiburg
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the University of Freiburg nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <octomap_msgs/conversions.h>
00032 #include <octomap/octomap.h>
00033 #include <fstream>
00034 
00035 #include <octomap_msgs/GetOctomap.h>
00036 using octomap_msgs::GetOctomap;
00037 
00038 #define USAGE "\nUSAGE: octomap_server_static <mapfile.[bt|ot]>\n" \
00039                 "  mapfile.bt: OctoMap filename to be loaded (.bt: binary tree, .ot: general octree)\n"
00040 
00041 using namespace std;
00042 using namespace octomap;
00043 
00044 class OctomapServerStatic{
00045 public:
00046   OctomapServerStatic(const std::string& filename)
00047     : m_octree(NULL), m_worldFrameId("/map")
00048   {
00049 
00050     ros::NodeHandle private_nh("~");
00051     private_nh.param("frame_id", m_worldFrameId, m_worldFrameId);
00052 
00053 
00054     // open file:
00055     if (filename.length() <= 3){
00056       ROS_ERROR("Octree file does not have .ot extension");
00057       exit(1);
00058     }
00059 
00060     std::string suffix = filename.substr(filename.length()-3, 3);
00061 
00062     // .bt files only as OcTree, all other classes need to be in .ot files:
00063     if (suffix == ".bt"){
00064       OcTree* octree = new OcTree(filename);
00065 
00066       m_octree = octree;
00067     } else if (suffix == ".ot"){
00068       AbstractOcTree* tree = AbstractOcTree::read(filename);
00069       if (!tree){
00070         ROS_ERROR("Could not read octree from file");
00071         exit(1);
00072       }
00073 
00074       m_octree = dynamic_cast<AbstractOccupancyOcTree*>(tree);
00075 
00076     } else{
00077       ROS_ERROR("Octree file does not have .bt or .ot extension");
00078       exit(1);
00079     }
00080 
00081     if (!m_octree ){
00082       ROS_ERROR("Could not read right octree class in file");
00083       exit(1);
00084     }
00085 
00086     ROS_INFO("Read octree type \"%s\" from file %s", m_octree->getTreeType().c_str(), filename.c_str());
00087     ROS_INFO("Octree resultion: %f, size: %zu", m_octree->getResolution(), m_octree->size());
00088 
00089 
00090     m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServerStatic::octomapBinarySrv, this);
00091     m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServerStatic::octomapFullSrv, this);
00092 
00093   }
00094 
00095   ~OctomapServerStatic(){
00096 
00097 
00098   }
00099 
00100   bool octomapBinarySrv(GetOctomap::Request  &req,
00101                         GetOctomap::Response &res)
00102   {
00103     ROS_INFO("Sending binary map data on service request");
00104     res.map.header.frame_id = m_worldFrameId;
00105     res.map.header.stamp = ros::Time::now();
00106     if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map))
00107       return false;
00108 
00109     return true;
00110   }
00111 
00112   bool octomapFullSrv(GetOctomap::Request  &req,
00113                                      GetOctomap::Response &res)
00114   {
00115     ROS_INFO("Sending full map data on service request");
00116     res.map.header.frame_id = m_worldFrameId;
00117     res.map.header.stamp = ros::Time::now();
00118 
00119 
00120     if (!octomap_msgs::fullMapToMsg(*m_octree, res.map))
00121       return false;
00122 
00123     return true;
00124   }
00125 
00126 private:
00127   ros::ServiceServer m_octomapBinaryService, m_octomapFullService;
00128   ros::NodeHandle m_nh;
00129   std::string m_worldFrameId;
00130   AbstractOccupancyOcTree* m_octree;
00131 
00132 };
00133 
00134 int main(int argc, char** argv){
00135   ros::init(argc, argv, "octomap_server_static");
00136   std::string mapFilename("");
00137 
00138   if (argc == 2)
00139     mapFilename = std::string(argv[1]);
00140   else{
00141     ROS_ERROR("%s", USAGE);
00142     exit(1);
00143   }
00144 
00145   try{
00146     OctomapServerStatic ms(mapFilename);
00147     ros::spin();
00148   }catch(std::runtime_error& e){
00149     ROS_ERROR("octomap_server_static exception: %s", e.what());
00150     exit(2);
00151   }
00152 
00153   exit(0);
00154 }
00155 
00156 


octomap_server
Author(s): Armin Hornung
autogenerated on Wed Nov 23 2016 03:40:03