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 
00047 OccupancyMapMonitor::OccupancyMapMonitor(double map_resolution) :
00048   map_resolution_(map_resolution),
00049   debug_info_(false),
00050   mesh_handle_count_(0),
00051   nh_("~"),
00052   active_(false)
00053 {
00054   initialize();
00055 }
00056 
00057 OccupancyMapMonitor::OccupancyMapMonitor(const boost::shared_ptr<tf::Transformer> &tf, const std::string &map_frame, double map_resolution) :
00058   tf_(tf),
00059   map_frame_(map_frame),
00060   map_resolution_(map_resolution),
00061   debug_info_(false),
00062   mesh_handle_count_(0),
00063   nh_("~")
00064 {
00065   initialize();
00066 }
00067 
00068 void OccupancyMapMonitor::initialize()
00069 {
00070   /* load params from param server */
00071   if (map_resolution_ <= std::numeric_limits<double>::epsilon())
00072     if (!nh_.getParam("octomap_resolution", map_resolution_))
00073     {
00074       map_resolution_ = 0.1;
00075       ROS_WARN("Resolution not specified for Octomap. Assuming resolution = %g instead", map_resolution_);
00076     }
00077   ROS_DEBUG("Using resolution = %lf m for building octomap", map_resolution_);
00078 
00079   if (map_frame_.empty())
00080     if (!nh_.getParam("octomap_frame", map_frame_))
00081       if (tf_)
00082         ROS_WARN("No target frame specified for Octomap. No transforms will be applied to received data.");
00083 
00084   if (!tf_ && !map_frame_.empty())
00085     ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data.");
00086 
00087   tree_.reset(new OccMapTree(map_resolution_));
00088   tree_const_ = tree_;
00089 
00090   XmlRpc::XmlRpcValue sensor_list;
00091   if (nh_.getParam("sensors", sensor_list))
00092   {
00093     try
00094     {
00095       if (sensor_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00096         for (int32_t i = 0; i < sensor_list.size(); ++i)
00097         {
00098           if (!sensor_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
00099           {
00100             ROS_ERROR("Params for octomap updater %d not a struct; ignoring.", i);
00101             continue;
00102           }
00103 
00104           if (!sensor_list[i].hasMember ("sensor_plugin"))
00105           {
00106             ROS_ERROR("No sensor plugin specified for octomap updater %d; ignoring.", i);
00107             continue;
00108           }
00109 
00110           std::string sensor_plugin = std::string(sensor_list[i]["sensor_plugin"]);
00111           if (sensor_plugin.empty() || sensor_plugin[0] == '~')
00112           {
00113             ROS_INFO("Skipping octomap updater plugin '%s'", sensor_plugin.c_str());
00114             continue;
00115           }
00116 
00117           if (!updater_plugin_loader_)
00118           {
00119             try
00120             {
00121               updater_plugin_loader_.reset(new pluginlib::ClassLoader<OccupancyMapUpdater>("moveit_ros_perception", "occupancy_map_monitor::OccupancyMapUpdater"));
00122             }
00123             catch(pluginlib::PluginlibException& ex)
00124             {
00125               ROS_FATAL_STREAM("Exception while creating octomap updater plugin loader " << ex.what());
00126             }
00127           }
00128 
00129           OccupancyMapUpdaterPtr up;
00130           try
00131           {
00132             up.reset(updater_plugin_loader_->createUnmanagedInstance(sensor_plugin));
00133             up->setMonitor(this);
00134           }
00135           catch(pluginlib::PluginlibException& ex)
00136           {
00137             ROS_ERROR_STREAM("Exception while loading octomap updater '" << sensor_plugin << "': " << ex.what() << 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(), sensor_plugin.c_str());
00151               continue;
00152             }
00153 
00154             addUpdater(up);
00155           }
00156         }
00157       else
00158         ROS_ERROR("List of sensors must be an array!");
00159     }
00160     catch (XmlRpc::XmlRpcException &ex)
00161     {
00162       ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
00163     }
00164   }
00165 
00166   /* advertise a service for loading octomaps from disk */
00167   save_map_srv_ = nh_.advertiseService("save_map", &OccupancyMapMonitor::saveMapCallback, this);
00168   load_map_srv_ = nh_.advertiseService("load_map", &OccupancyMapMonitor::loadMapCallback, this);
00169 }
00170 
00171 void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr &updater)
00172 {
00173   if (updater)
00174   {
00175     map_updaters_.push_back(updater);
00176     updater->publishDebugInformation(debug_info_);
00177     if (map_updaters_.size() > 1)
00178     {
00179       mesh_handles_.resize(map_updaters_.size());
00180       if (map_updaters_.size() == 2) // when we had one updater only, we passed direcly the transform cache callback to that updater
00181       {
00182         map_updaters_[0]->setTransformCacheCallback(boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3));
00183         map_updaters_[1]->setTransformCacheCallback(boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3));
00184       }
00185       else
00186         map_updaters_.back()->setTransformCacheCallback(boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3));
00187     }
00188     else
00189       updater->setTransformCacheCallback(transform_cache_callback_);
00190   }
00191   else
00192     ROS_ERROR("NULL updater was specified");
00193 }
00194 
00195 void OccupancyMapMonitor::publishDebugInformation(bool flag)
00196 {
00197   debug_info_ = flag;
00198   for (std::size_t i = 0 ; i < map_updaters_.size() ; ++i)
00199     map_updaters_[i]->publishDebugInformation(debug_info_);
00200 }
00201 
00202 void OccupancyMapMonitor::setMapFrame(const std::string &frame)
00203 {
00204   boost::mutex::scoped_lock _(parameters_lock_); // we lock since an updater could specify a new frame for us
00205   map_frame_ = frame;
00206 }
00207 
00208 ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr &shape)
00209 {
00210   // if we have just one updater, remove the additional level of indirection
00211   if (map_updaters_.size() == 1)
00212     return map_updaters_[0]->excludeShape(shape);
00213 
00214   ShapeHandle h = 0;
00215   for (std::size_t i = 0 ; i < map_updaters_.size() ; ++i)
00216   {
00217     ShapeHandle mh = map_updaters_[i]->excludeShape(shape);
00218     if (mh)
00219     {
00220       if (h == 0)
00221         h = ++mesh_handle_count_;
00222       mesh_handles_[i][h] = mh;
00223     }
00224   }
00225   return h;
00226 }
00227 
00228 void OccupancyMapMonitor::forgetShape(ShapeHandle handle)
00229 {
00230   // if we have just one updater, remove the additional level of indirection
00231   if (map_updaters_.size() == 1)
00232   {
00233     map_updaters_[0]->forgetShape(handle);
00234     return;
00235   }
00236 
00237   for (std::size_t i = 0 ; i < map_updaters_.size() ; ++i)
00238   {
00239     std::map<ShapeHandle, ShapeHandle>::const_iterator it = mesh_handles_[i].find(handle);
00240     if (it == mesh_handles_[i].end())
00241       continue;
00242     map_updaters_[i]->forgetShape(it->second);
00243   }
00244 }
00245 
00246 void OccupancyMapMonitor::setTransformCacheCallback(const TransformCacheProvider& transform_callback)
00247 {
00248   // if we have just one updater, we connect it directly to the transform provider
00249   if (map_updaters_.size() == 1)
00250     map_updaters_[0]->setTransformCacheCallback(transform_callback);
00251   else
00252     transform_cache_callback_ = transform_callback;
00253 }
00254 
00255 bool OccupancyMapMonitor::getShapeTransformCache(std::size_t index, const std::string &target_frame, const ros::Time &target_time, ShapeTransformCache &cache) const
00256 {
00257   if (transform_cache_callback_)
00258   {
00259     ShapeTransformCache tempCache;
00260     if (transform_cache_callback_(target_frame, target_time, tempCache))
00261     {
00262       for (ShapeTransformCache::iterator it = tempCache.begin() ; it != tempCache.end() ; ++it)
00263       {
00264         std::map<ShapeHandle, ShapeHandle>::const_iterator jt = mesh_handles_[index].find(it->first);
00265         if (jt == mesh_handles_[index].end())
00266         {
00267           ROS_ERROR_THROTTLE(1, "Incorrect mapping of mesh handles");
00268           return false;
00269         }
00270         else
00271           cache[jt->second] = it->second;
00272       }
00273       return true;
00274     }
00275     else
00276       return false;
00277   }
00278   else
00279     return false;
00280 }
00281 
00282 bool OccupancyMapMonitor::saveMapCallback(moveit_msgs::SaveMap::Request& request, moveit_msgs::SaveMap::Response& response)
00283 {
00284   ROS_INFO("Writing map to %s", request.filename.c_str());
00285   tree_->lockRead();
00286   try
00287   {
00288     response.success = tree_->writeBinary(request.filename);
00289   }
00290   catch (...)
00291   {
00292     response.success = false;
00293   }
00294   tree_->unlockRead();
00295   return true;
00296 }
00297 
00298 bool OccupancyMapMonitor::loadMapCallback(moveit_msgs::LoadMap::Request& request, moveit_msgs::LoadMap::Response& response)
00299 {
00300   ROS_INFO("Reading map from %s", request.filename.c_str());
00301 
00302   /* load the octree from disk */
00303   tree_->lockWrite();
00304   try
00305   {
00306     response.success = tree_->readBinary(request.filename);
00307   }
00308   catch (...)
00309   {
00310     ROS_ERROR("Failed to load map from file");
00311     response.success = false;
00312   }
00313   tree_->unlockWrite();
00314 
00315   return true;
00316 }
00317 
00318 void OccupancyMapMonitor::startMonitor()
00319 {
00320   active_ = true;
00321   /* initialize all of the occupancy map updaters */
00322   for (std::size_t i = 0 ; i < map_updaters_.size() ; ++i)
00323     map_updaters_[i]->start();
00324 }
00325 
00326 void OccupancyMapMonitor::stopMonitor()
00327 {
00328   active_ = false;
00329   for (std::size_t i = 0 ; i < map_updaters_.size() ; ++i)
00330     map_updaters_[i]->stop();
00331 }
00332 
00333 OccupancyMapMonitor::~OccupancyMapMonitor()
00334 {
00335   stopMonitor();
00336 }
00337 
00338 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Aug 26 2015 12:43:21