TrackingOctomapServer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, D. Kuhner, P. Ruchti, 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 
31 #include <string>
32 
33 using namespace octomap;
34 
35 namespace octomap_server {
36 
37 TrackingOctomapServer::TrackingOctomapServer(const std::string& filename) :
39 {
40  //read tree if necessary
41  if (filename != "") {
42  if (m_octree->readBinary(filename)) {
43  ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(), m_octree->size());
46  m_gridmap.info.resolution = m_res;
47 
48  publishAll();
49  } else {
50  ROS_ERROR("Could not open requested file %s, exiting.", filename.c_str());
51  exit(-1);
52  }
53  }
54 
55  ros::NodeHandle private_nh("~");
56 
57  std::string changeSetTopic = "changes";
58  std::string changeIdFrame = "/talker/changes";
59 
60  private_nh.param("topic_changes", changeSetTopic, changeSetTopic);
61  private_nh.param("change_id_frame", change_id_frame, changeIdFrame);
62  private_nh.param("track_changes", track_changes, false);
63  private_nh.param("listen_changes", listen_changes, false);
64  private_nh.param("min_change_pub", min_change_pub, 0);
65 
67  ROS_WARN("OctoMapServer: It might not be useful to publish changes and at the same time listen to them."
68  "Setting 'track_changes' to false!");
69  track_changes = false;
70  }
71 
72  if (track_changes) {
73  ROS_INFO("starting server");
74  pubChangeSet = private_nh.advertise<sensor_msgs::PointCloud2>(
75  changeSetTopic, 1);
77  }
78 
79  if (listen_changes) {
80  ROS_INFO("starting client");
81  subChangeSet = private_nh.subscribe(changeSetTopic, 1,
83  }
84 }
85 
87 }
88 
89 void TrackingOctomapServer::insertScan(const tf::Point & sensorOrigin, const PCLPointCloud & ground, const PCLPointCloud & nonground) {
90  OctomapServer::insertScan(sensorOrigin, ground, nonground);
91 
92  if (track_changes) {
93  trackChanges();
94  }
95 }
96 
98  KeyBoolMap::const_iterator startPnt = m_octree->changedKeysBegin();
99  KeyBoolMap::const_iterator endPnt = m_octree->changedKeysEnd();
100 
101  pcl::PointCloud<pcl::PointXYZI> changedCells = pcl::PointCloud<pcl::PointXYZI>();
102 
103  int c = 0;
104  for (KeyBoolMap::const_iterator iter = startPnt; iter != endPnt; ++iter) {
105  ++c;
106  OcTreeNode* node = m_octree->search(iter->first);
107 
108  bool occupied = m_octree->isNodeOccupied(node);
109 
110  point3d center = m_octree->keyToCoord(iter->first);
111 
112  pcl::PointXYZI pnt;
113  pnt.x = center(0);
114  pnt.y = center(1);
115  pnt.z = center(2);
116 
117  if (occupied) {
118  pnt.intensity = 1000;
119  }
120  else {
121  pnt.intensity = -1000;
122  }
123 
124  changedCells.push_back(pnt);
125  }
126 
127  if (c > min_change_pub)
128  {
129  sensor_msgs::PointCloud2 changed;
130  pcl::toROSMsg(changedCells, changed);
131  changed.header.frame_id = change_id_frame;
132  changed.header.stamp = ros::Time().now();
133  pubChangeSet.publish(changed);
134  ROS_DEBUG("[server] sending %d changed entries", (int)changedCells.size());
135 
137  ROS_DEBUG("[server] octomap size after updating: %d", (int)m_octree->calcNumNodes());
138  }
139 }
140 
141 void TrackingOctomapServer::trackCallback(sensor_msgs::PointCloud2Ptr cloud) {
142  pcl::PointCloud<pcl::PointXYZI> cells;
143  pcl::fromROSMsg(*cloud, cells);
144  ROS_DEBUG("[client] size of newly occupied cloud: %i", (int)cells.points.size());
145 
146  for (size_t i = 0; i < cells.points.size(); i++) {
147  pcl::PointXYZI& pnt = cells.points[i];
148  m_octree->updateNode(m_octree->coordToKey(pnt.x, pnt.y, pnt.z), pnt.intensity, false);
149  }
150 
152  ROS_DEBUG("[client] octomap size after updating: %d", (int)m_octree->calcNumNodes());
153 }
154 
155 } /* namespace octomap */
void insertScan(const tf::Point &sensorOrigin, const PCLPointCloud &ground, const PCLPointCloud &nonground)
update occupancy map with a scan labeled as ground and nonground. The scans should be in the global m...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
double keyToCoord(key_type key, unsigned depth) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
KeyBoolMap::const_iterator changedKeysEnd() const
key_type coordToKey(double coordinate) const
void trackCallback(sensor_msgs::PointCloud2Ptr cloud)
tf::Vector3 Point
#define ROS_WARN(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
OcTreeNode * search(double x, double y, double z, unsigned int depth=0) const
#define ROS_INFO(...)
KeyBoolMap::const_iterator changedKeysBegin() const
virtual void insertScan(const tf::Point &sensorOrigin, const PCLPointCloud &ground, const PCLPointCloud &nonground)
update occupancy map with a scan labeled as ground and nonground. The scans should be in the global m...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void publishAll(const ros::Time &rostime=ros::Time::now())
bool readBinary(std::istream &s)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
static Time now()
void enableChangeDetection(bool enable)
nav_msgs::OccupancyGrid m_gridmap
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
pcl::PointCloud< pcl::PointXYZ > PCLPointCloud
Definition: OctomapServer.h:90
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


octomap_server
Author(s): Armin Hornung
autogenerated on Tue Dec 27 2022 03:15:28