octomap_server_static.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, A. Hornung, University of Freiburg
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the University of Freiburg nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
32 #include <octomap/octomap.h>
33 #include <fstream>
34 
35 #include <octomap_msgs/GetOctomap.h>
36 using octomap_msgs::GetOctomap;
37 
38 #define USAGE "\nUSAGE: octomap_server_static <mapfile.[bt|ot]>\n" \
39  " mapfile.bt: OctoMap filename to be loaded (.bt: binary tree, .ot: general octree)\n"
40 
41 using namespace std;
42 using namespace octomap;
43 
45 public:
46  OctomapServerStatic(const std::string& filename)
47  : m_octree(NULL), m_worldFrameId("/map")
48  {
49 
50  ros::NodeHandle private_nh("~");
51  private_nh.param("frame_id", m_worldFrameId, m_worldFrameId);
52 
53 
54  // open file:
55  if (filename.length() <= 3){
56  ROS_ERROR("Octree file does not have .ot extension");
57  exit(1);
58  }
59 
60  std::string suffix = filename.substr(filename.length()-3, 3);
61 
62  // .bt files only as OcTree, all other classes need to be in .ot files:
63  if (suffix == ".bt"){
64  OcTree* octree = new OcTree(filename);
65 
66  m_octree = octree;
67  } else if (suffix == ".ot"){
68  AbstractOcTree* tree = AbstractOcTree::read(filename);
69  if (!tree){
70  ROS_ERROR("Could not read octree from file");
71  exit(1);
72  }
73 
74  m_octree = dynamic_cast<AbstractOccupancyOcTree*>(tree);
75 
76  } else{
77  ROS_ERROR("Octree file does not have .bt or .ot extension");
78  exit(1);
79  }
80 
81  if (!m_octree ){
82  ROS_ERROR("Could not read right octree class in file");
83  exit(1);
84  }
85 
86  ROS_INFO("Read octree type \"%s\" from file %s", m_octree->getTreeType().c_str(), filename.c_str());
87  ROS_INFO("Octree resultion: %f, size: %zu", m_octree->getResolution(), m_octree->size());
88 
89 
90  m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServerStatic::octomapBinarySrv, this);
91  m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServerStatic::octomapFullSrv, this);
92 
93  }
94 
96 
97 
98  }
99 
100  bool octomapBinarySrv(GetOctomap::Request &req,
101  GetOctomap::Response &res)
102  {
103  ROS_INFO("Sending binary map data on service request");
104  res.map.header.frame_id = m_worldFrameId;
105  res.map.header.stamp = ros::Time::now();
106  if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map))
107  return false;
108 
109  return true;
110  }
111 
112  bool octomapFullSrv(GetOctomap::Request &req,
113  GetOctomap::Response &res)
114  {
115  ROS_INFO("Sending full map data on service request");
116  res.map.header.frame_id = m_worldFrameId;
117  res.map.header.stamp = ros::Time::now();
118 
119 
120  if (!octomap_msgs::fullMapToMsg(*m_octree, res.map))
121  return false;
122 
123  return true;
124  }
125 
126 private:
127  ros::ServiceServer m_octomapBinaryService, m_octomapFullService;
129  std::string m_worldFrameId;
131 
132 };
133 
134 int main(int argc, char** argv){
135  ros::init(argc, argv, "octomap_server_static");
136  std::string mapFilename("");
137 
138  if (argc == 2)
139  mapFilename = std::string(argv[1]);
140  else{
141  ROS_ERROR("%s", USAGE);
142  exit(1);
143  }
144 
145  try{
146  OctomapServerStatic ms(mapFilename);
147  ros::spin();
148  }catch(std::runtime_error& e){
149  ROS_ERROR("octomap_server_static exception: %s", e.what());
150  exit(2);
151  }
152 
153  exit(0);
154 }
155 
156 
OctomapServerStatic::octomapFullSrv
bool octomapFullSrv(GetOctomap::Request &req, GetOctomap::Response &res)
Definition: octomap_server_static.cpp:112
main
int main(int argc, char **argv)
Definition: octomap_server_static.cpp:134
OctomapServerStatic::m_octomapFullService
ros::ServiceServer m_octomapFullService
Definition: octomap_server_static.cpp:127
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
OctomapServerStatic::~OctomapServerStatic
~OctomapServerStatic()
Definition: octomap_server_static.cpp:95
ros::ServiceServer
OctomapServerStatic::OctomapServerStatic
OctomapServerStatic(const std::string &filename)
Definition: octomap_server_static.cpp:46
octomap::OcTree
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
OctomapServerStatic
Definition: octomap_server_static.cpp:44
octomap::AbstractOcTree
OctomapServerStatic::m_nh
ros::NodeHandle m_nh
Definition: octomap_server_static.cpp:128
OctomapServerStatic::m_worldFrameId
std::string m_worldFrameId
Definition: octomap_server_static.cpp:129
std
octomap::AbstractOccupancyOcTree
ROS_ERROR
#define ROS_ERROR(...)
octomap.h
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
USAGE
#define USAGE
Definition: octomap_server_static.cpp:38
OctomapServerStatic::m_octree
AbstractOccupancyOcTree * m_octree
Definition: octomap_server_static.cpp:130
conversions.h
OctomapServerStatic::octomapBinarySrv
bool octomapBinarySrv(GetOctomap::Request &req, GetOctomap::Response &res)
Definition: octomap_server_static.cpp:100
ros::spin
ROSCPP_DECL void spin()
octomap
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle
ros::Time::now
static Time now()


octomap_server
Author(s): Armin Hornung
autogenerated on Mon Jan 2 2023 03:18:15