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


occupancy_map_monitor
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed May 5 2021 02:53:56