world_model.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_plugins/plugin_aggregators/world_model.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 WorldModel::WorldModel()
00006   : ExtendedPluginAggregator<WorldModel, CollisionCheckPlugin>("WorldModel")
00007 {
00008 }
00009 
00010 void WorldModel::loadPlugins()
00011 {
00012   ExtendedPluginAggregator<WorldModel, CollisionCheckPlugin>::loadPlugins();
00013 
00014   // get terrain model
00015   vigir_pluginlib::PluginManager::getPlugin(terrain_model_);
00016   if (terrain_model_)
00017   {
00018     ROS_INFO("[WorldModel] Found terrain model:");
00019     ROS_INFO("    %s (%s)", terrain_model_->getName().c_str(), terrain_model_->getTypeClass().c_str());
00020   }
00021 }
00022 
00023 bool WorldModel::loadParams(const vigir_generic_params::ParameterSet& params)
00024 {
00025   bool result = ExtendedPluginAggregator<WorldModel, CollisionCheckPlugin>::loadParams(params);
00026 
00027   if (terrain_model_)
00028     result &= terrain_model_->loadParams(params);
00029 
00030   return result;
00031 }
00032 
00033 void WorldModel::resetPlugins()
00034 {
00035   ExtendedPluginAggregator<WorldModel, CollisionCheckPlugin>::resetPlugins();
00036 
00037   if (terrain_model_)
00038     terrain_model_->reset();
00039 }
00040 
00041 bool WorldModel::isAccessible(const State& s) const
00042 {
00043   for (CollisionCheckPlugin::Ptr plugin : getPlugins())
00044   {
00045     if (plugin && plugin->isCollisionCheckAvailable() && !plugin->isAccessible(s))
00046       return false;
00047   }
00048   return true;
00049 }
00050 
00051 bool WorldModel::isAccessible(const State& next, const State& current) const
00052 {
00053   for (CollisionCheckPlugin::Ptr plugin : getPlugins())
00054   {
00055     if (plugin && plugin->isCollisionCheckAvailable() && !plugin->isAccessible(next, current))
00056       return false;
00057   }
00058   return true;
00059 }
00060 
00061 void WorldModel::useTerrainModel(bool enabled)
00062 {
00063   use_terrain_model_ = enabled;
00064 }
00065 
00066 bool WorldModel::isTerrainModelAvailable() const
00067 {
00068   return terrain_model_ && terrain_model_->isTerrainModelAvailable();
00069 }
00070 
00071 TerrainModelPlugin::ConstPtr WorldModel::getTerrainModel() const
00072 {
00073   return terrain_model_;
00074 }
00075 
00076 bool WorldModel::getHeight(double x, double y, double& height) const
00077 {
00078   if (!use_terrain_model_ || !isTerrainModelAvailable())
00079     return true;
00080 
00081   return terrain_model_->getHeight(x, y, height);
00082 }
00083 
00084 bool WorldModel::update3DData(State& s) const
00085 {
00086   if (!use_terrain_model_ || !isTerrainModelAvailable())
00087     return true;
00088 
00089   return terrain_model_->update3DData(s);
00090 }
00091 }


vigir_footstep_planning_plugins
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:48:02