$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: but_server.h 2537 2013-02-21 15:57:57Z stancl $ 00005 * 00006 * Modified by dcgm-robotics@FIT group 00007 * 00008 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00009 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00010 * Date: dd/mm/2012 00011 * 00012 * This code is derived from the OctoMap server provided by A. Hornung. 00013 * Please, see the original comments below. 00014 */ 00015 00053 #pragma once 00054 #ifndef SERVER_H 00055 #define SERVER_H 00056 00057 #include <ros/ros.h> 00058 #include <visualization_msgs/MarkerArray.h> 00059 #include <std_msgs/ColorRGBA.h> 00060 00061 //#include <sensor_msgs/CameraInfo.h> 00062 //#include <std_srvs/Empty.h> 00063 00064 //#include <pcl/point_types.h> 00065 //#include <pcl/io/pcd_io.h> 00066 00067 #include <srs_env_model/but_server/plugins/cmap_plugin.h> 00068 #include <srs_env_model/but_server/plugins/point_cloud_plugin.h> 00069 #include <srs_env_model/but_server/plugins/octomap_plugin.h> 00070 #include <srs_env_model/but_server/plugins/collision_object_plugin.h> 00071 #include <srs_env_model/but_server/plugins/imarkers_plugin.h> 00072 #include <srs_env_model/but_server/plugins/marker_array_plugin.h> 00073 #include <srs_env_model/but_server/plugins/limited_point_cloud_plugin.h> 00074 #include <srs_env_model/but_server/plugins/compressed_point_cloud_plugin.h> 00075 #include <srs_env_model/but_server/plugins/objtree_plugin.h> 00076 #include <srs_env_model/but_server/plugins/collision_grid_plugin.h> 00077 00078 #include <srs_env_model/ButServerPause.h> 00079 #include <srs_env_model/UseInputColor.h> 00080 00081 // Old interactive markers plugin used for testing 00082 #include <srs_env_model/but_server/plugins/old_imarkers_plugin.h> 00083 00084 #define _EXAMPLES_ 00085 #ifdef _EXAMPLES_ 00086 # include <srs_env_model/but_server/plugins/example_plugin.h> 00087 #endif 00088 00089 00090 namespace srs_env_model 00091 { 00092 00096 class CButServer 00097 { 00098 public: 00100 typedef pcl::PointCloud<pcl::PointXYZ> tPCLPointCloud; 00101 00103 CButServer(const std::string& filename= ""); 00104 00106 virtual ~CButServer(); 00107 00109 void reset(); 00110 00112 void pause( bool bPause ); 00113 00114 00115 protected: 00117 void publishAll(const ros::Time& rostime ); 00118 00120 void onOcMapDataChanged( tButServerOcMapConstPtr mapdata, const ros::Time & stamp ) 00121 { 00122 publishAll(stamp); 00123 } 00124 00126 bool onReset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) 00127 { 00128 reset(); 00129 return true; 00130 } 00131 00133 bool onPause( ButServerPause::Request & request, ButServerPause::Response & response ); 00134 00136 void publishPlugins(const ros::Time& rostime); 00137 00139 bool onUseInputColor(srs_env_model::UseInputColor::Request & req, srs_env_model::UseInputColor::Response & res ); 00140 00141 00142 protected: 00144 bool m_bIsPaused; 00145 00147 ros::NodeHandle m_nh; 00148 00149 bool m_latchedTopics; 00150 00152 int m_numPCFramesProcessed; 00153 00155 int m_frameCounter; 00156 00157 //====================================================================================================== 00158 // Services 00159 00161 ros::ServiceServer m_serviceReset; 00162 00164 ros::ServiceServer m_servicePause; 00165 00167 ros::ServiceServer m_serviceUseInputColor; 00168 00169 //====================================================================================================== 00170 // Plugins 00171 00173 typedef std::vector<CServerPluginBase * > tVecPlugins; 00174 00176 tVecPlugins m_plugins; 00177 00179 #define FOR_ALL_PLUGINS( X ) { for( tVecPlugins::iterator p = m_plugins.begin(); p != m_plugins.end(); ++p ){ (*p)->X; } } 00180 #define FOR_ALL_PLUGINS_PARAM( X, Y ) { for( tVecPlugins::iterator p = m_plugins.begin(); p != m_plugins.end(); ++p ){ (*p)->X(Y); } } 00181 00183 boost::shared_ptr< CCMapPlugin > m_plugCMap; 00184 00186 boost::shared_ptr< CPointCloudPlugin > m_plugInputPointCloud, 00188 m_plugOcMapPointCloud; 00190 boost::shared_ptr< CPointCloudPlugin > m_plugVisiblePointCloud; 00191 00193 boost::shared_ptr< COctoMapPlugin > m_plugOctoMap; 00194 00196 boost::shared_ptr< CCollisionObjectPlugin > m_plugCollisionObject; 00197 00199 boost::shared_ptr< CCollisionGridPlugin > m_plugMap2D; 00200 00202 boost::shared_ptr< CIMarkersPlugin > m_plugIMarkers; 00203 00205 boost::shared_ptr< CMarkerArrayPlugin > m_plugMarkerArray; 00206 00208 boost::shared_ptr< CObjTreePlugin > m_plugObjTree; 00209 00211 boost::shared_ptr< COldIMarkersPlugin > m_plugOldIMarkers; 00212 00214 boost::shared_ptr< CCompressedPointCloudPlugin > m_plugCompressedPointCloud; 00215 00217 bool m_bUseOldIMP; 00218 00219 #ifdef _EXAMPLES_ 00220 00221 boost::shared_ptr< CExamplePlugin > m_plugExample; 00222 00224 boost::shared_ptr< CExampleCrawlerPlugin > m_plugExampleCrawler; 00225 #endif 00226 }; 00227 00228 00229 } // namespace srs_env_model 00230 00231 #endif // SERVER_H