occupancy_map_server.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Jon Binney, Ioan Sucan */
36 
37 #include <boost/shared_ptr.hpp>
38 #include <ros/ros.h>
39 #include <tf/tf.h>
40 #include <tf/transform_listener.h>
43 
45 {
46  octomap_msgs::Octomap map;
47 
48  map.header.frame_id = server->getMapFrame();
49  map.header.stamp = ros::Time::now();
50 
51  server->getOcTreePtr()->lockRead();
52  try
53  {
54  if (!octomap_msgs::binaryMapToMsgData(*server->getOcTreePtr(), map.data))
55  ROS_ERROR_THROTTLE(1, "Could not generate OctoMap message");
56  }
57  catch (...)
58  {
59  ROS_ERROR_THROTTLE(1, "Exception thrown while generating OctoMap message");
60  }
61  server->getOcTreePtr()->unlockRead();
62 
63  octree_binary_pub->publish(map);
64 }
65 
66 int main(int argc, char** argv)
67 {
68  ros::init(argc, argv, "occupancy_map_server");
69  ros::NodeHandle nh;
70  ros::Publisher octree_binary_pub = nh.advertise<octomap_msgs::Octomap>("octomap_binary", 1);
71  boost::shared_ptr<tf::Transformer> listener = boost::make_shared<tf::TransformListener>(ros::Duration(5.0));
73  server.setUpdateCallback(boost::bind(&publishOctomap, &octree_binary_pub, &server));
74  server.startMonitor();
75 
76  ros::spin();
77  return 0;
78 }
void publish(const boost::shared_ptr< M > &message) const
static void publishOctomap(ros::Publisher *octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor *server)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_ERROR_THROTTLE(period,...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const OccMapTreePtr & getOcTreePtr()
Get a pointer to the underlying octree for this monitor. Lock the tree before reading or writing usin...
static Time now()
int main(int argc, char **argv)
static bool binaryMapToMsgData(const OctomapT &octomap, std::vector< int8_t > &mapData)


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Jul 10 2019 04:03:27