/**
* Publish data
*/
void srs_env_model::CButServer::publishPlugins(const ros::Time& rostime)
{
FOR_ALL_PLUGINS_PARAM( publish, rostime );
}
/**
Constructor
*/
srs_env_model::CButServer::CButServer(const std::string& filename) :
m_bIsPaused(false),
m_nh(),
m_latchedTopics(false),
m_numPCFramesProcessed(1.0), m_frameCounter(0),
m_plugCMap(new CCMapPlugin("CMAP" )),
m_plugInputPointCloud( new CPointCloudPlugin("PCIN", true ) ),
m_plugOcMapPointCloud( new CPointCloudPlugin( "PCOC", false )),
m_plugVisiblePointCloud( new CLimitedPointCloudPlugin( "PCVIS" ) ),
m_plugOctoMap( new COctoMapPlugin("OCM", filename)),
m_plugCollisionObject( new CCollisionObjectPlugin( "COB" )),
m_plugMap2D( new CCollisionGridPlugin( "M2D" )),
m_plugIMarkers( ),
m_plugMarkerArray( new CMarkerArrayPlugin( "MA" ) ),
m_plugObjTree( new CObjTreePlugin( "OT" ) ),
m_plugOldIMarkers(),
m_plugCompressedPointCloud( new CCompressedPointCloudPlugin( "CPC" ) ),
m_bUseOldIMP( false )
#ifdef _EXAMPLES_
, m_plugExample( new CExamplePlugin("Example1") )
, m_plugExampleCrawler( new CExampleCrawlerPlugin("Example2") )
#endif
{
// Get node handle
ros::NodeHandle private_nh("~");
// Advertise services
m_serviceReset = private_nh.advertiseService(ServerReset_SRV, &CButServer::onReset, this);
m_servicePause = private_nh.advertiseService(ServerPause_SRV, &CButServer::onPause, this);
m_latchedTopics = false;
private_nh.param("latch", m_latchedTopics, m_latchedTopics);
private_nh.param<bool>("use_old_im", m_bUseOldIMP, m_bUseOldIMP);
std::cerr << "BUTSERVER: Initializing plugins " << std::endl;
// Store all plugins pointers for easier access
m_plugins.push_back( m_plugCMap.get() );
m_plugins.push_back( m_plugInputPointCloud.get() );
m_plugins.push_back( m_plugOcMapPointCloud.get() );
m_plugins.push_back( m_plugVisiblePointCloud.get() );
m_plugins.push_back( m_plugOctoMap.get() );
m_plugins.push_back( m_plugCollisionObject.get() );
m_plugins.push_back( m_plugMap2D.get() );
m_plugins.push_back( m_plugMarkerArray.get() );
m_plugins.push_back( m_plugObjTree.get() );
m_plugins.push_back( m_plugCompressedPointCloud.get() );
if( m_bUseOldIMP )
{
m_plugOldIMarkers = boost::shared_ptr< COldIMarkersPlugin >(new COldIMarkersPlugin( "IM" ) );
m_plugins.push_back( m_plugOldIMarkers.get() );
}
else
{
m_plugIMarkers = boost::shared_ptr< CIMarkersPlugin >( new CIMarkersPlugin( "IM" ) );
m_plugins.push_back( m_plugIMarkers.get() );
}
#ifdef _EXAMPLES_
m_plugins.push_back( m_plugExample.get() );
m_plugins.push_back( m_plugExampleCrawler.get() );
#endif
//=========================================================================
// Initialize plugins
FOR_ALL_PLUGINS_PARAM(init, private_nh)
std::cerr << "BUTSERVER: All plugins initialized. Starting server. " << std::endl;
// Connect input point cloud input with octomap
m_plugInputPointCloud->getSigDataChanged().connect( boost::bind( &COctoMapPlugin::insertCloud, m_plugOctoMap, _1 ));
// Connect all crawlers
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCMapPlugin::handleNewMapData, m_plugCMap, _1 ) );
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CPointCloudPlugin::handleNewMapData, m_plugOcMapPointCloud, _1 ) );
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCollisionGridPlugin::handleNewMapData, m_plugMap2D, _1 ) );
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CMarkerArrayPlugin::handleNewMapData, m_plugMarkerArray, _1 ) );
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCompressedPointCloudPlugin::handleNewMapData, m_plugCompressedPointCloud, _1 ) );
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CCollisionObjectPlugin::handleNewMapData, m_plugCollisionObject, _1 ) );
m_plugOctoMap->getSigOnNewData().connect( boost::bind( &CLimitedPointCloudPlugin::handleNewMapData, m_plugVisiblePointCloud, _1 ) );
// Connect octomap data changed signal with server publish
m_plugOctoMap->getSigDataChanged().connect( boost::bind( &CButServer::onOcMapDataChanged, *this, _1, _2 ));
} // Constructor
/**
Constructor
*/
CButServer::CButServer(const std::string& filename) :
m_nh(),
m_latchedTopics(false),
m_numPCFramesProcessed(1.0), m_frameCounter(0),
m_plugCMapHolder("CMAP"),
m_plugInputPointCloudHolder("PCIN"),
m_plugOcMapPointCloudHolder("PCOC"),
m_plugVisiblePointCloudHolder("PCVIS"),
m_plugOctoMap("OCM"),
m_plugCollisionObjectHolder("COB"),
m_plugMap2DHolder("M2D"),
m_plugIMarkers(0),
m_plugMarkerArrayHolder( "MA" ),
m_plugObjTree( "OT" ),
m_plugOldIMarkers( 0 ),
m_bUseOldIMP( false )
#ifdef _EXAMPLES_
, m_plugExample("Example1")
, m_plugExampleCrawlerHolder( "Example2")
#endif
{
// Get node handle
ros::NodeHandle private_nh("~");
// Advertise services
m_serviceReset = private_nh.advertiseService("ButServerReset", &CButServer::onReset, this);
// Is map static (loaded from file)?
bool staticMap(filename != "");
m_latchedTopics = staticMap;
private_nh.param("latch", m_latchedTopics, m_latchedTopics);
private_nh.param<bool>("use_old_im", m_bUseOldIMP, m_bUseOldIMP);
std::cerr << "BUTSERVER: Initializing plugins " << std::endl;
// Store all plugins pointer for easier access
m_plugins.push_back( m_plugCMapHolder.getPlugin() );
m_plugins.push_back( m_plugInputPointCloudHolder.getPlugin() );
m_plugins.push_back( m_plugOcMapPointCloudHolder.getPlugin() );
m_plugins.push_back( m_plugVisiblePointCloudHolder.getPlugin() );
m_plugins.push_back( &m_plugOctoMap );
m_plugins.push_back( m_plugCollisionObjectHolder.getPlugin() );
m_plugins.push_back( m_plugMap2DHolder.getPlugin() );
m_plugins.push_back( m_plugMarkerArrayHolder.getPlugin() );
m_plugins.push_back( &m_plugObjTree );
if( m_bUseOldIMP )
{
m_plugOldIMarkers = new srs::COldIMarkersPlugin( "IM" );
m_plugins.push_back( m_plugOldIMarkers );
}
else
{
m_plugIMarkers = new srs::CIMarkersPlugin( "IM" );
m_plugins.push_back( m_plugIMarkers );
}
#ifdef _EXAMPLES_
m_plugins.push_back( & m_plugExample );
m_plugins.push_back( m_plugExampleCrawlerHolder.getPlugin() );
#endif
//=========================================================================
// Initialize plugins
FOR_ALL_PLUGINS_PARAM(init, private_nh)
std::cerr << "BUTSERVER: All plugins initialized. Starting server. " << std::endl;
// Connect input point cloud input with octomap
m_plugInputPointCloudHolder.getPlugin()->getSigDataChanged().connect( boost::bind( &srs::COctoMapPlugin::insertCloud, &m_plugOctoMap, _1 ));
// Connect octomap data changed signal with server publish
m_plugOctoMap.getSigDataChanged().connect( boost::bind( &CButServer::onOcMapDataChanged, this, _1 ));
} // Constructor
