but_server.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  * $Id: but_server.cpp 809 2012-05-19 21:47:48Z stancl $
00004  *
00005  * Modified by dcgm-robotics@FIT group.
00006  *
00007  * Author: Vit Stancl (stancl@fit.vutbr.cz)
00008  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00009  * Date: dd.mm.2011
00010  *
00011  * This code is derived from the OctoMap server provided by A. Hornung.
00012  * Please, see the original comments below.
00013  */
00014 
00052 #include <srs_env_model/but_server/but_server.h>
00053 #include <srs_env_model/topics_list.h>
00054 //#include <srs_env_model/but_server/server_tools.h>
00055 
00056 #include <sstream>
00057 
00059 //template <typename tpType> void swap( tpType & x, tpType & y){ tpType b(x); x = y; y = b; }
00060 
00062 
00065 srs_env_model::CButServer::CButServer(const std::string& filename) :
00066                         m_bIsPaused(false),
00067                         m_nh(),
00068                         m_latchedTopics(false),
00069                         m_numPCFramesProcessed(1.0), m_frameCounter(0),
00070                         m_plugCMap(new CCMapPlugin("CMAP" )),
00071                         m_plugInputPointCloud( new CPointCloudPlugin("PCIN", true ) ),
00072                         m_plugOcMapPointCloud( new CPointCloudPlugin( "PCOC", false )),
00073 //                      m_plugVisiblePointCloud( new CLimitedPointCloudPlugin( "PCVIS" ) ),
00074                         m_plugOctoMap( new COctoMapPlugin("OCM", filename)),
00075                         m_plugCollisionObject( new CCollisionObjectPlugin( "COB" )),
00076                         m_plugMap2D( new CCollisionGridPlugin( "M2D" )),
00077                         m_plugIMarkers( ),
00078                         m_plugMarkerArray( new CMarkerArrayPlugin( "MA" ) ),
00079                         m_plugObjTree( new CObjTreePlugin( "OT" ) ),
00080                         m_plugOldIMarkers( ),
00081                         m_plugCompressedPointCloud( new CCompressedPointCloudPlugin( "CPC" ) ),
00082                         m_bUseOldIMP( false )
00083 #ifdef _EXAMPLES_
00084                         , m_plugExample( new CExamplePlugin("Example1") )
00085                         , m_plugExampleCrawler( new CExampleCrawlerPlugin("Example2") )
00086 #endif
00087 
00088 {
00089         // Get node handle
00090         ros::NodeHandle private_nh("~");
00091 
00092         // Advertise services
00093         m_serviceReset = private_nh.advertiseService(ServerReset_SRV, &CButServer::onReset, this);
00094         m_servicePause = private_nh.advertiseService(ServerPause_SRV, &CButServer::onPause, this);
00095         m_serviceUseInputColor = private_nh.advertiseService( ServerUseInputColor_SRV, &CButServer::onUseInputColor, this);
00096 
00097 
00098         m_latchedTopics = false;
00099         private_nh.param("latch", m_latchedTopics, m_latchedTopics);
00100         private_nh.param<bool>("use_old_im", m_bUseOldIMP, m_bUseOldIMP);
00101 
00102         std::cerr << "BUTSERVER: Initializing plugins " << std::endl;
00103 
00104         // Store all plugins pointers for easier access
00105         m_plugins.push_back( m_plugCMap.get() );
00106         m_plugins.push_back( m_plugInputPointCloud.get() );
00107         m_plugins.push_back( m_plugOcMapPointCloud.get() );
00108 //      m_plugins.push_back( m_plugVisiblePointCloud.get() );
00109         m_plugins.push_back( m_plugOctoMap.get() );
00110         m_plugins.push_back( m_plugCollisionObject.get() );
00111         m_plugins.push_back( m_plugMap2D.get() );
00112         m_plugins.push_back( m_plugMarkerArray.get() );
00113         m_plugins.push_back( m_plugObjTree.get() );
00114         m_plugins.push_back( m_plugCompressedPointCloud.get() );
00115 
00116         if( m_bUseOldIMP )
00117         {
00118                 m_plugOldIMarkers = boost::shared_ptr< COldIMarkersPlugin >(new COldIMarkersPlugin( "IM"  ) );
00119                 m_plugins.push_back( m_plugOldIMarkers.get() );
00120         }
00121         else
00122         {
00123                 m_plugIMarkers = boost::shared_ptr< CIMarkersPlugin >( new CIMarkersPlugin( "IM" ) );
00124                 m_plugins.push_back( m_plugIMarkers.get() );
00125         }
00126 
00127 #ifdef _EXAMPLES_
00128         m_plugins.push_back( m_plugExample.get() );
00129         m_plugins.push_back( m_plugExampleCrawler.get() );
00130 #endif
00131 
00132         //=========================================================================
00133         // Initialize plugins
00134         FOR_ALL_PLUGINS_PARAM(init, private_nh)
00135 
00136         std::cerr << "BUTSERVER: All plugins initialized. Starting server. " << std::endl;
00137 
00138         // Connect input point cloud input with octomap
00139         m_plugInputPointCloud->getSigDataChanged().connect( boost::bind( &COctoMapPlugin::insertCloud, m_plugOctoMap, _1 ));
00140 
00141         // Connect all crawlers
00142         m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCMapPlugin::handleNewMapData, m_plugCMap, _1 ) );
00143         m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CPointCloudPlugin::handleNewMapData, m_plugOcMapPointCloud, _1 ) );
00144         m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCollisionGridPlugin::handleNewMapData, m_plugMap2D, _1 ) );
00145         m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CMarkerArrayPlugin::handleNewMapData, m_plugMarkerArray, _1 ) );
00146         m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCompressedPointCloudPlugin::handleNewMapData, m_plugCompressedPointCloud, _1 ) );
00147         m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCollisionObjectPlugin::handleNewMapData, m_plugCollisionObject, _1 ) );
00148 //      m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CLimitedPointCloudPlugin::handleNewMapData, m_plugVisiblePointCloud, _1 ) );
00149 
00150         // Connect octomap data changed signal with server publish
00151         m_plugOctoMap->getSigDataChanged().connect( boost::bind( &CButServer::onOcMapDataChanged, *this, _1, _2 ));
00152 
00153 } // Constructor
00154 
00156 
00159 srs_env_model::CButServer::~CButServer()
00160 {
00161 
00162 }
00163 
00164 
00165 
00167 
00171 void srs_env_model::CButServer::publishAll(const ros::Time& rostime)
00172 {
00173         // Store start time
00174 //      ros::WallTime startTime = ros::WallTime::now();
00175 
00176         // If no data, do nothing
00177         if (m_plugOctoMap->getSize() <= 1) {
00178                 ROS_WARN("Nothing to publish, octree is empty");
00179                 return;
00180         }
00181 
00182         // init markers:
00183         visualization_msgs::MarkerArray occupiedNodesVis;
00184 
00185         // each array stores all cubes of a different size, one for each depth level:
00186         occupiedNodesVis.markers.resize(m_plugOctoMap->getTreeDepth() + 1);
00187 
00188         //=========================================================================
00189         // Plugins frame start
00190 
00191         // Crawl octomap
00192         m_plugOctoMap->crawl( rostime );
00193 
00194         publishPlugins( rostime );
00195 
00196         // Compute and show elapsed time
00197 //      double total_elapsed = (ros::WallTime::now() - startTime).toSec();
00198 //      ROS_DEBUG("Map publishing in CButServer took %f sec", total_elapsed);
00199 }
00200 
00201 
00203 
00206 void srs_env_model::CButServer::reset()
00207 {
00208   ROS_DEBUG("Reseting environment server...");
00209 
00210   FOR_ALL_PLUGINS(reset());
00211 
00212   ROS_DEBUG("Environment server reset finished.");
00213 
00214 }
00215 
00217 
00220 bool srs_env_model::CButServer::onPause( ButServerPause::Request & request, ButServerPause::Response & response )
00221 {
00222         std::cerr << "On pause" << std::endl;
00223 
00224         if( request.pause == 0 )
00225                 pause( false );
00226         else
00227                 pause( true );
00228 
00229 
00230         return m_bIsPaused;
00231 }
00232 
00234 
00237 void srs_env_model::CButServer::pause( bool bPause )
00238 {
00239         if( m_bIsPaused == bPause )
00240                 return;
00241 
00242         // Get node handle
00243         ros::NodeHandle private_nh("~");
00244 
00245         tVecPlugins::iterator p, end( m_plugins.end() );
00246 
00247         for( p = m_plugins.begin(); p != end; ++p )
00248         {
00249                 (*p)->pause( bPause, private_nh );
00250         }
00251 
00252         if( bPause )
00253                 std::cerr << "BUT server paused..." << std::endl;
00254         else
00255                 std::cerr << "BUT server resumed..." << std::endl;
00256 
00257         m_bIsPaused = bPause;
00258 }
00259 
00260 
00261 
00263 
00266 void srs_env_model::CButServer::publishPlugins(const ros::Time& rostime)
00267 {
00268         FOR_ALL_PLUGINS_PARAM( publish, rostime );
00269 
00270 }
00271 
00273 
00276 bool srs_env_model::CButServer::onUseInputColor(srs_env_model::UseInputColor::Request & req, srs_env_model::UseInputColor::Response & res )
00277 {
00278         // Set value to the pc input plugin
00279         m_plugInputPointCloud->setUseInputColor(req.use_color);
00280 
00281         return true;
00282 }
00283 


srs_env_model
Author(s): Vit Stancl (stancl@fit.vutbr.cz), Tomas Lokaj, Jan Gorig, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Mon Oct 6 2014 08:05:04