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  ROS_ERROR("Failed to find 3D sensor plugin parameters for octomap generation");
178 
179  /* advertise a service for loading octomaps from disk */
182 }
183 
184 void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater)
185 {
186  if (updater)
187  {
188  map_updaters_.push_back(updater);
189  updater->publishDebugInformation(debug_info_);
190  if (map_updaters_.size() > 1)
191  {
192  mesh_handles_.resize(map_updaters_.size());
193  // when we had one updater only, we passed direcly the transform cache callback to that updater
194  if (map_updaters_.size() == 2)
195  {
196  map_updaters_[0]->setTransformCacheCallback(
197  boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3));
198  map_updaters_[1]->setTransformCacheCallback(
199  boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3));
200  }
201  else
202  map_updaters_.back()->setTransformCacheCallback(
203  boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3));
204  }
205  else
206  updater->setTransformCacheCallback(transform_cache_callback_);
207  }
208  else
209  ROS_ERROR("NULL updater was specified");
210 }
211 
213 {
214  debug_info_ = flag;
215  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
217 }
218 
219 void OccupancyMapMonitor::setMapFrame(const std::string& frame)
220 {
221  boost::mutex::scoped_lock _(parameters_lock_); // we lock since an updater could specify a new frame for us
222  map_frame_ = frame;
223 }
224 
226 {
227  // if we have just one updater, remove the additional level of indirection
228  if (map_updaters_.size() == 1)
229  return map_updaters_[0]->excludeShape(shape);
230 
231  ShapeHandle h = 0;
232  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
233  {
234  ShapeHandle mh = map_updaters_[i]->excludeShape(shape);
235  if (mh)
236  {
237  if (h == 0)
238  h = ++mesh_handle_count_;
239  mesh_handles_[i][h] = mh;
240  }
241  }
242  return h;
243 }
244 
246 {
247  // if we have just one updater, remove the additional level of indirection
248  if (map_updaters_.size() == 1)
249  {
250  map_updaters_[0]->forgetShape(handle);
251  return;
252  }
253 
254  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
255  {
256  std::map<ShapeHandle, ShapeHandle>::const_iterator it = mesh_handles_[i].find(handle);
257  if (it == mesh_handles_[i].end())
258  continue;
259  map_updaters_[i]->forgetShape(it->second);
260  }
261 }
262 
264 {
265  // if we have just one updater, we connect it directly to the transform provider
266  if (map_updaters_.size() == 1)
267  map_updaters_[0]->setTransformCacheCallback(transform_callback);
268  else
269  transform_cache_callback_ = transform_callback;
270 }
271 
272 bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::string& target_frame,
273  const ros::Time& target_time, ShapeTransformCache& cache) const
274 {
276  {
277  ShapeTransformCache temp_cache;
278  if (transform_cache_callback_(target_frame, target_time, temp_cache))
279  {
280  for (ShapeTransformCache::iterator it = temp_cache.begin(); it != temp_cache.end(); ++it)
281  {
282  std::map<ShapeHandle, ShapeHandle>::const_iterator jt = mesh_handles_[index].find(it->first);
283  if (jt == mesh_handles_[index].end())
284  {
285  ROS_ERROR_THROTTLE(1, "Incorrect mapping of mesh handles");
286  return false;
287  }
288  else
289  cache[jt->second] = it->second;
290  }
291  return true;
292  }
293  else
294  return false;
295  }
296  else
297  return false;
298 }
299 
300 bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request,
301  moveit_msgs::SaveMap::Response& response)
302 {
303  ROS_INFO("Writing map to %s", request.filename.c_str());
304  tree_->lockRead();
305  try
306  {
307  response.success = tree_->writeBinary(request.filename);
308  }
309  catch (...)
310  {
311  response.success = false;
312  }
313  tree_->unlockRead();
314  return true;
315 }
316 
317 bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request,
318  moveit_msgs::LoadMap::Response& response)
319 {
320  ROS_INFO("Reading map from %s", request.filename.c_str());
321 
322  /* load the octree from disk */
323  tree_->lockWrite();
324  try
325  {
326  response.success = tree_->readBinary(request.filename);
327  }
328  catch (...)
329  {
330  ROS_ERROR("Failed to load map from file");
331  response.success = false;
332  }
333  tree_->unlockWrite();
334 
335  if (response.success)
336  tree_->triggerUpdateCallback();
337 
338  return true;
339 }
340 
342 {
343  active_ = true;
344  /* initialize all of the occupancy map updaters */
345  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
346  map_updaters_[i]->start();
347 }
348 
350 {
351  active_ = false;
352  for (std::size_t i = 0; i < map_updaters_.size(); ++i)
353  map_updaters_[i]->stop();
354 }
355 
357 {
358  stopMonitor();
359 }
360 } // 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 Thu Jan 14 2021 03:59:05