00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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)
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_);
00205 map_frame_ = frame;
00206 }
00207
00208 ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr &shape)
00209 {
00210
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
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
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
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
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 }