occupancy_map_monitor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Jon Binney */
00036 
00037 #include <ros/ros.h>
00038 #include <moveit_msgs/SaveMap.h>
00039 #include <moveit_msgs/LoadMap.h>
00040 #include <moveit/occupancy_map_monitor/occupancy_map.h>
00041 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00042 #include <XmlRpcException.h>
00043 
00044 namespace occupancy_map_monitor
00045 {
00046 OccupancyMapMonitor::OccupancyMapMonitor(double map_resolution)
00047   : map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_("~"), active_(false)
00048 {
00049   initialize();
00050 }
00051 
00052 OccupancyMapMonitor::OccupancyMapMonitor(const boost::shared_ptr<tf::Transformer>& tf, const std::string& map_frame,
00053                                          double map_resolution)
00054   : tf_(tf), map_frame_(map_frame), map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_("~")
00055 {
00056   initialize();
00057 }
00058 
00059 OccupancyMapMonitor::OccupancyMapMonitor(const boost::shared_ptr<tf::Transformer>& tf, ros::NodeHandle& nh,
00060                                          const std::string& map_frame, double map_resolution)
00061   : tf_(tf), map_frame_(map_frame), map_resolution_(map_resolution), debug_info_(false), mesh_handle_count_(0), nh_(nh)
00062 {
00063   initialize();
00064 }
00065 
00066 void OccupancyMapMonitor::initialize()
00067 {
00068   /* load params from param server */
00069   if (map_resolution_ <= std::numeric_limits<double>::epsilon())
00070     if (!nh_.getParam("octomap_resolution", map_resolution_))
00071     {
00072       map_resolution_ = 0.1;
00073       ROS_WARN("Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_);
00074     }
00075   ROS_DEBUG("Using resolution = %lf m for building octomap", map_resolution_);
00076 
00077   if (map_frame_.empty())
00078     if (!nh_.getParam("octomap_frame", map_frame_))
00079       if (tf_)
00080         ROS_WARN("No target frame specified for Octomap. No transforms will be applied to received data.");
00081 
00082   if (!tf_ && !map_frame_.empty())
00083     ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data.");
00084 
00085   tree_.reset(new OccMapTree(map_resolution_));
00086   tree_const_ = tree_;
00087 
00088   XmlRpc::XmlRpcValue sensor_list;
00089   if (nh_.getParam("sensors", sensor_list))
00090   {
00091     try
00092     {
00093       if (sensor_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00094         for (int32_t i = 0; i < sensor_list.size(); ++i)
00095         {
00096           if (!sensor_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
00097           {
00098             ROS_ERROR("Params for octomap updater %d not a struct; ignoring.", i);
00099             continue;
00100           }
00101 
00102           if (!sensor_list[i].hasMember("sensor_plugin"))
00103           {
00104             ROS_ERROR("No sensor plugin specified for octomap updater %d; ignoring.", i);
00105             continue;
00106           }
00107 
00108           std::string sensor_plugin = std::string(sensor_list[i]["sensor_plugin"]);
00109           if (sensor_plugin.empty() || sensor_plugin[0] == '~')
00110           {
00111             ROS_INFO("Skipping octomap updater plugin '%s'", sensor_plugin.c_str());
00112             continue;
00113           }
00114 
00115           if (!updater_plugin_loader_)
00116           {
00117             try
00118             {
00119               updater_plugin_loader_.reset(new pluginlib::ClassLoader<OccupancyMapUpdater>(
00120                   "moveit_ros_perception", "occupancy_map_monitor::OccupancyMapUpdater"));
00121             }
00122             catch (pluginlib::PluginlibException& ex)
00123             {
00124               ROS_FATAL_STREAM("Exception while creating octomap updater plugin loader " << ex.what());
00125             }
00126           }
00127 
00128           OccupancyMapUpdaterPtr up;
00129           try
00130           {
00131             up.reset(updater_plugin_loader_->createUnmanagedInstance(sensor_plugin));
00132             up->setMonitor(this);
00133           }
00134           catch (pluginlib::PluginlibException& ex)
00135           {
00136             ROS_ERROR_STREAM("Exception while loading octomap updater '" << sensor_plugin << "': " << ex.what()
00137                                                                          << std::endl);
00138           }
00139           if (up)
00140           {
00141             /* pass the params struct directly in to the updater */
00142             if (!up->setParams(sensor_list[i]))
00143             {
00144               ROS_ERROR("Failed to configure updater of type %s", up->getType().c_str());
00145               continue;
00146             }
00147 
00148             if (!up->initialize())
00149             {
00150               ROS_ERROR("Unable to initialize map updater of type %s (plugin %s)", up->getType().c_str(),
00151                         sensor_plugin.c_str());
00152               continue;
00153             }
00154 
00155             addUpdater(up);
00156           }
00157         }
00158       else
00159         ROS_ERROR("List of sensors must be an array!");
00160     }
00161     catch (XmlRpc::XmlRpcException& ex)
00162     {
00163       ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
00164     }
00165   }
00166 
00167   /* advertise a service for loading octomaps from disk */
00168   save_map_srv_ = nh_.advertiseService("save_map", &OccupancyMapMonitor::saveMapCallback, this);
00169   load_map_srv_ = nh_.advertiseService("load_map", &OccupancyMapMonitor::loadMapCallback, this);
00170 }
00171 
00172 void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater)
00173 {
00174   if (updater)
00175   {
00176     map_updaters_.push_back(updater);
00177     updater->publishDebugInformation(debug_info_);
00178     if (map_updaters_.size() > 1)
00179     {
00180       mesh_handles_.resize(map_updaters_.size());
00181       if (map_updaters_.size() ==
00182           2)  // when we had one updater only, we passed direcly the transform cache callback to that updater
00183       {
00184         map_updaters_[0]->setTransformCacheCallback(
00185             boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3));
00186         map_updaters_[1]->setTransformCacheCallback(
00187             boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3));
00188       }
00189       else
00190         map_updaters_.back()->setTransformCacheCallback(
00191             boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3));
00192     }
00193     else
00194       updater->setTransformCacheCallback(transform_cache_callback_);
00195   }
00196   else
00197     ROS_ERROR("NULL updater was specified");
00198 }
00199 
00200 void OccupancyMapMonitor::publishDebugInformation(bool flag)
00201 {
00202   debug_info_ = flag;
00203   for (std::size_t i = 0; i < map_updaters_.size(); ++i)
00204     map_updaters_[i]->publishDebugInformation(debug_info_);
00205 }
00206 
00207 void OccupancyMapMonitor::setMapFrame(const std::string& frame)
00208 {
00209   boost::mutex::scoped_lock _(parameters_lock_);  // we lock since an updater could specify a new frame for us
00210   map_frame_ = frame;
00211 }
00212 
00213 ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr& shape)
00214 {
00215   // if we have just one updater, remove the additional level of indirection
00216   if (map_updaters_.size() == 1)
00217     return map_updaters_[0]->excludeShape(shape);
00218 
00219   ShapeHandle h = 0;
00220   for (std::size_t i = 0; i < map_updaters_.size(); ++i)
00221   {
00222     ShapeHandle mh = map_updaters_[i]->excludeShape(shape);
00223     if (mh)
00224     {
00225       if (h == 0)
00226         h = ++mesh_handle_count_;
00227       mesh_handles_[i][h] = mh;
00228     }
00229   }
00230   return h;
00231 }
00232 
00233 void OccupancyMapMonitor::forgetShape(ShapeHandle handle)
00234 {
00235   // if we have just one updater, remove the additional level of indirection
00236   if (map_updaters_.size() == 1)
00237   {
00238     map_updaters_[0]->forgetShape(handle);
00239     return;
00240   }
00241 
00242   for (std::size_t i = 0; i < map_updaters_.size(); ++i)
00243   {
00244     std::map<ShapeHandle, ShapeHandle>::const_iterator it = mesh_handles_[i].find(handle);
00245     if (it == mesh_handles_[i].end())
00246       continue;
00247     map_updaters_[i]->forgetShape(it->second);
00248   }
00249 }
00250 
00251 void OccupancyMapMonitor::setTransformCacheCallback(const TransformCacheProvider& transform_callback)
00252 {
00253   // if we have just one updater, we connect it directly to the transform provider
00254   if (map_updaters_.size() == 1)
00255     map_updaters_[0]->setTransformCacheCallback(transform_callback);
00256   else
00257     transform_cache_callback_ = transform_callback;
00258 }
00259 
00260 bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::string& target_frame,
00261                                                  const ros::Time& target_time, ShapeTransformCache& cache) const
00262 {
00263   if (transform_cache_callback_)
00264   {
00265     ShapeTransformCache tempCache;
00266     if (transform_cache_callback_(target_frame, target_time, tempCache))
00267     {
00268       for (ShapeTransformCache::iterator it = tempCache.begin(); it != tempCache.end(); ++it)
00269       {
00270         std::map<ShapeHandle, ShapeHandle>::const_iterator jt = mesh_handles_[index].find(it->first);
00271         if (jt == mesh_handles_[index].end())
00272         {
00273           ROS_ERROR_THROTTLE(1, "Incorrect mapping of mesh handles");
00274           return false;
00275         }
00276         else
00277           cache[jt->second] = it->second;
00278       }
00279       return true;
00280     }
00281     else
00282       return false;
00283   }
00284   else
00285     return false;
00286 }
00287 
00288 bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request,
00289                                           moveit_msgs::SaveMap::Response& response)
00290 {
00291   ROS_INFO("Writing map to %s", request.filename.c_str());
00292   tree_->lockRead();
00293   try
00294   {
00295     response.success = tree_->writeBinary(request.filename);
00296   }
00297   catch (...)
00298   {
00299     response.success = false;
00300   }
00301   tree_->unlockRead();
00302   return true;
00303 }
00304 
00305 bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request,
00306                                           moveit_msgs::LoadMap::Response& response)
00307 {
00308   ROS_INFO("Reading map from %s", request.filename.c_str());
00309 
00310   /* load the octree from disk */
00311   tree_->lockWrite();
00312   try
00313   {
00314     response.success = tree_->readBinary(request.filename);
00315   }
00316   catch (...)
00317   {
00318     ROS_ERROR("Failed to load map from file");
00319     response.success = false;
00320   }
00321   tree_->unlockWrite();
00322 
00323   return true;
00324 }
00325 
00326 void OccupancyMapMonitor::startMonitor()
00327 {
00328   active_ = true;
00329   /* initialize all of the occupancy map updaters */
00330   for (std::size_t i = 0; i < map_updaters_.size(); ++i)
00331     map_updaters_[i]->start();
00332 }
00333 
00334 void OccupancyMapMonitor::stopMonitor()
00335 {
00336   active_ = false;
00337   for (std::size_t i = 0; i < map_updaters_.size(); ++i)
00338     map_updaters_[i]->stop();
00339 }
00340 
00341 OccupancyMapMonitor::~OccupancyMapMonitor()
00342 {
00343   stopMonitor();
00344 }
00345 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Jun 19 2019 19:24:12