occupancy_map_monitor.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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: Ioan Sucan, Jon Binney */
36 
37 #include <ros/ros.h>
38 #include <moveit_msgs/SaveMap.h>
39 #include <moveit_msgs/LoadMap.h>
42 #include <XmlRpcException.h>
43 
44 namespace occupancy_map_monitor
45 {
47  : map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_("~"), active_(false)
48 {
49  initialize();
50 }
51 
53  double map_resolution)
54  : tf_(tf), map_frame_(map_frame), map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_("~")
55 {
56  initialize();
57 }
58 
60  const std::string& map_frame, double map_resolution)
61  : tf_(tf), map_frame_(map_frame), map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_(nh)
62 {
63  initialize();
64 }
65 
67 {
68  /* load params from param server */
69  if (map_resolution_ <= std::numeric_limits<double>::epsilon())
70  if (!nh_.getParam("octomap_resolution", map_resolution_))
71  {
72  map_resolution_ = 0.1;
73  ROS_WARN("Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_);
74  }
75  ROS_DEBUG("Using resolution = %lf m for building octomap", map_resolution_);
76 
77  if (map_frame_.empty())
78  if (!nh_.getParam("octomap_frame", map_frame_))
79  if (tf_)
80  ROS_WARN("No target frame specified for Octomap. No transforms will be applied to received data.");
81 
82  if (!tf_ && !map_frame_.empty())
83  ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data.");
84 
85  tree_.reset(new OccMapTree(map_resolution_));
87 
88  XmlRpc::XmlRpcValue sensor_list;
89  if (nh_.getParam("sensors", sensor_list))
90  {
91  try
92  {
93  if (sensor_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
94  for (int32_t i = 0; i < sensor_list.size(); ++i)
95  {
96  if (sensor_list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
97  {
98  ROS_ERROR("Params for octomap updater %d not a struct; ignoring.", i);
99  continue;
100  }
101 
102  if (!sensor_list[i].hasMember("sensor_plugin"))
103  {
104  ROS_ERROR("No sensor plugin specified for octomap updater %d; ignoring.", i);
105  continue;
106  }
107 
108  std::string sensor_plugin = std::string(sensor_list[i]["sensor_plugin"]);
109  if (sensor_plugin.empty() || sensor_plugin[0] == '~')
110  {
111  ROS_INFO("Skipping octomap updater plugin '%s'", sensor_plugin.c_str());
112  continue;
113  }
114 
116  {
117  try
118  {
120  "moveit_ros_perception", "occupancy_map_monitor::OccupancyMapUpdater"));
121  }
123  {
124  ROS_FATAL_STREAM("Exception while creating octomap updater plugin loader " << ex.what());
125  }
126  }
127 
128  OccupancyMapUpdaterPtr up;
129  try
130  {
131  up.reset(updater_plugin_loader_->createUnmanagedInstance(sensor_plugin));
132  up->setMonitor(this);
133  }
135  {
136  ROS_ERROR_STREAM("Exception while loading octomap updater '" << sensor_plugin << "': " << ex.what()
137  << std::endl);
138  }
139  if (up)
140  {
141  /* pass the params struct directly in to the updater */
142  if (!up->setParams(sensor_list[i]))
143  {
144  ROS_ERROR("Failed to configure updater of type %s", up->getType().c_str());
145  continue;
146  }
147 
148  if (!up->initialize())
149  {
150  ROS_ERROR("Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(),
151  sensor_plugin.c_str());
152  continue;
153  }
154 
155  addUpdater(up);
156  }
157  }
158  else
159  ROS_ERROR("List of sensors must be an array!");
160  }
161  catch (XmlRpc::XmlRpcException& ex)
162  {
163  ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
164  }
165  }
166 
167  /* advertise a service for loading octomaps from disk */
170 }
171 
172 void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater)
173 {
174  if (updater)
175  {
176  map_updaters_.push_back(updater);
177  updater->publishDebugInformation(debug_info_);
178  if (map_updaters_.size() > 1)
179  {
180  mesh_handles_.resize(map_updaters_.size());
181  if (map_updaters_.size() ==
182  2) // when we had one updater only, we passed direcly the transform cache callback to that updater
183  {
184  map_updaters_[0]->setTransformCacheCallback(
185  boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3));
186  map_updaters_[1]->setTransformCacheCallback(
187  boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3));
188  }
189  else
190  map_updaters_.back()->setTransformCacheCallback(
191  boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3));
192  }
193  else
194  updater->setTransformCacheCallback(transform_cache_callback_);
195  }
196  else
197  ROS_ERROR("NULL updater was specified");
198 }
199 
201 {
202  debug_info_ = flag;
203  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
205 }
206 
207 void OccupancyMapMonitor::setMapFrame(const std::string& frame)
208 {
209  boost::mutex::scoped_lock _(parameters_lock_); // we lock since an updater could specify a new frame for us
210  map_frame_ = frame;
211 }
212 
214 {
215  // if we have just one updater, remove the additional level of indirection
216  if (map_updaters_.size() == 1)
217  return map_updaters_[0]->excludeShape(shape);
218 
219  ShapeHandle h = 0;
220  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
221  {
222  ShapeHandle mh = map_updaters_[i]->excludeShape(shape);
223  if (mh)
224  {
225  if (h == 0)
226  h = ++mesh_handle_count_;
227  mesh_handles_[i][h] = mh;
228  }
229  }
230  return h;
231 }
232 
234 {
235  // if we have just one updater, remove the additional level of indirection
236  if (map_updaters_.size() == 1)
237  {
238  map_updaters_[0]->forgetShape(handle);
239  return;
240  }
241 
242  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
243  {
244  std::map<ShapeHandle, ShapeHandle>::const_iterator it = mesh_handles_[i].find(handle);
245  if (it == mesh_handles_[i].end())
246  continue;
247  map_updaters_[i]->forgetShape(it->second);
248  }
249 }
250 
252 {
253  // if we have just one updater, we connect it directly to the transform provider
254  if (map_updaters_.size() == 1)
255  map_updaters_[0]->setTransformCacheCallback(transform_callback);
256  else
257  transform_cache_callback_ = transform_callback;
258 }
259 
260 bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::string& target_frame,
261  const ros::Time& target_time, ShapeTransformCache& cache) const
262 {
264  {
265  ShapeTransformCache tempCache;
266  if (transform_cache_callback_(target_frame, target_time, tempCache))
267  {
268  for (ShapeTransformCache::iterator it = tempCache.begin(); it != tempCache.end(); ++it)
269  {
270  std::map<ShapeHandle, ShapeHandle>::const_iterator jt = mesh_handles_[index].find(it->first);
271  if (jt == mesh_handles_[index].end())
272  {
273  ROS_ERROR_THROTTLE(1, "Incorrect mapping of mesh handles");
274  return false;
275  }
276  else
277  cache[jt->second] = it->second;
278  }
279  return true;
280  }
281  else
282  return false;
283  }
284  else
285  return false;
286 }
287 
288 bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request,
289  moveit_msgs::SaveMap::Response& response)
290 {
291  ROS_INFO("Writing map to %s", request.filename.c_str());
292  tree_->lockRead();
293  try
294  {
295  response.success = tree_->writeBinary(request.filename);
296  }
297  catch (...)
298  {
299  response.success = false;
300  }
301  tree_->unlockRead();
302  return true;
303 }
304 
305 bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request,
306  moveit_msgs::LoadMap::Response& response)
307 {
308  ROS_INFO("Reading map from %s", request.filename.c_str());
309 
310  /* load the octree from disk */
311  tree_->lockWrite();
312  try
313  {
314  response.success = tree_->readBinary(request.filename);
315  }
316  catch (...)
317  {
318  ROS_ERROR("Failed to load map from file");
319  response.success = false;
320  }
321  tree_->unlockWrite();
322 
323  return true;
324 }
325 
327 {
328  active_ = true;
329  /* initialize all of the occupancy map updaters */
330  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
331  map_updaters_[i]->start();
332 }
333 
335 {
336  active_ = false;
337  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
338  map_updaters_[i]->stop();
339 }
340 
342 {
343  stopMonitor();
344 }
345 }
const std::string & getMessage() const
std::map< ShapeHandle, Eigen::Affine3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Affine3d > > > ShapeTransformCache
void setTransformCacheCallback(const TransformCacheProvider &transform_cache_callback)
int size() const
std::unique_ptr< pluginlib::ClassLoader< OccupancyMapUpdater > > updater_plugin_loader_
boost::shared_ptr< tf::Transformer > tf_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
#define ROS_WARN(...)
std::vector< std::map< ShapeHandle, ShapeHandle > > mesh_handles_
bool loadMapCallback(moveit_msgs::LoadMap::Request &request, moveit_msgs::LoadMap::Response &response)
Load octree from a binary file (gets rid of current octree data)
#define ROS_ERROR_THROTTLE(rate,...)
bool saveMapCallback(moveit_msgs::SaveMap::Request &request, moveit_msgs::SaveMap::Response &response)
Save the current octree to a binary file.
#define ROS_FATAL_STREAM(args)
void forgetShape(ShapeHandle handle)
Forget about this shape handle and the shapes it corresponds to.
#define ROS_INFO(...)
void addUpdater(const OccupancyMapUpdaterPtr &updater)
void startMonitor()
start the monitor (will begin updating the octomap
boost::function< bool(const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache)> TransformCacheProvider
std::vector< OccupancyMapUpdaterPtr > map_updaters_
bool getShapeTransformCache(std::size_t index, const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache) const
bool getParam(const std::string &key, std::string &s) const
OccupancyMapMonitor(const boost::shared_ptr< tf::Transformer > &tf, const std::string &map_frame="", double map_resolution=0.0)
ShapeHandle excludeShape(const shapes::ShapeConstPtr &shape)
Add this shape to the set of shapes to be filtered out from the octomap.
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)
std::shared_ptr< const Shape > ShapeConstPtr


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23