| Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials, ros.org. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Using srs_env_model
Description: This tutorial introduces SRS environment model concepts, and covers writing simple server plugins.Tutorial Level: BEGINNER
Contents
Introduction
Environment model server consists of server node and plugins. All functionality is hidden in plugins. Currently there are two main functionality parts - part based on incoming 3D scans processing and objtree which serves as a recognized objects manager. To add any new functionality plugin must be written.
Server plugin how-to
Simple plugin
To write server plugin (in C++) you must inherit class from srs_env_model::CServerPluginBase class (see declaration in include/srs_env_model/but_server/server_tools.h). This class defines basic plugin interface for a whole plugin life - initialization, data publishing control and resetting. After that, you must add your plugin header file to the but_server.cpp file, add a new plugin variable and connect this variable to the server node. See example below:
Plugin header file:
1 namespace srs
2 {
3
4 /**
5 * Simple plugin - just explaining what methods do...
6 */
7 class CExamplePlugin : public CServerPluginBase
8 {
9 public:
10 //! Constructor. Plugin name can be used for debug string output.
11 CExamplePlugin( const std::string & name = "ExamplePlugin ")
12 : CServerPluginBase( name )
13 {}
14
15 //! This method is called by server on start. Use node_handle to do
16 //! parameters reading and all initializations here (subscriber
17 //! connection, publisher creation, etc.)
18 virtual void init(ros::NodeHandle & node_handle){}
19
20 //! New scan has been inserted in the octomap. After that
21 //! publishing stage starts and if this node has something to
22 //! publish (see last method), this method is called.
23 virtual void onPublish(const ros::Time & timestamp){}
24
25 //! This method is called when user wants to reset server.
26 virtual void reset() {}
27
28 //! Return true, if you want to publish something now. Called
29 //! before onPublish method.
30 virtual bool shouldPublish(){ return false; }
31
32 }; // class CExamplePlugin
33
34 } // namespace srs
35
Now we must connect this plugin to the server. File but_server.h:
Add right plugin constructor to the server constructor (if needed) and add it to the plugins vector (that vector is used to call all common methods. File but_server.cpp:
Now add publishing to the server's publishAll() method:
That is all for simple plugin.
Plugin containing data
If you want to hold some data in the plugin, you can add srs_env_model::CDataHolderBase to your inheritance list. This template declares some useful functions - get data reference, data validation and invalidation and at last signal, that is called when data was invalidated. Interface is prepared as a template. The template argument is stored data type. See example of plugin in the next chapter.
Octomap "crawling" plugin
The third class (also templated one) is srs_env_model::COctomapCrawlerBase. This need some further explanation of Octomap plugin functionality. This plugin stores incoming scans in the octomap data form. After scan insertion the whole octomap is crawled - it is iterated and connected signals are called - on "crawling" start, on each node,on the free and the occupied node and when all this is finished. You can connect your plugin to this process if you implement "crawler" interface and if you connect to the octomap plugin. At first the "Crawler" example:
1 /**
2 * Simple crawler plugin example.
3 * As a crawler template parameter use octomap node type. Additionally this
4 * plugin uses data interface to store color used in handleOccupiedNode method.
5 */
6 class CExampleCrawlerPlugin
7 : public CServerPluginBase
8 , public COctomapCrawlerBase<tButServerOcTree::NodeType>
9 , public CDataHolderBase<unsigned char[3]>
10 {
11 public:
12 //! Constructor. Plugin name can be used for debug string output.
13 CExampleCrawlerPlugin( const std::string & name = "ExamplePlugin ")
14 : CServerPluginBase( name )
15 {
16 // Store some data
17 (*m_data)[0] = 255; (*m_data)[0] = 0; (*m_data)[0] = 0;
18
19 }
20
21 /// Basic plugin methods are the same
22 virtual void init(ros::NodeHandle & node_handle){}
23 virtual void onPublish(const ros::Time & timestamp){}
24 virtual void reset() {}
25 virtual bool shouldPublish(){ return false; }
26
27 //! Called before octomap traversing starts.
28 //! Set used octomap frame id and timestamp.
29 virtual void onFrameStart( const SMapParameters & par )
30 { m_frame_id = par.frameId; m_time_stamp = par.currentTime; }
31
32 //! Handle free node - set its color to the green.
33 virtual void handleFreeNode(tButServerOcTree::iterator & it, const SMapParameters & mp )
34 {
35 it->r() = 0;
36 it->g() = 255;
37 it->b() = 0;
38 }
39
40 /// Hook that is called when traversing all nodes of the updated Octree (does nothing here)
41 virtual void handleNode(srs_env_model::tButServerOcTree::iterator& it, const SMapParameters & mp) {};
42
43 /// Hook that is called when traversing occupied nodes of the updated Octree.
44 /// We set node color to the stored one.
45 virtual void handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapParameters & mp)
46 {
47 it->r() = (*m_data)[0];
48 it->g() = (*m_data)[1];
49 it->b() = (*m_data)[2];
50 }
51
52 /// Called when all nodes was visited.
53 virtual void handlePostNodeTraversal(const SMapParameters & mp){}
54
55
56 }; // class CExampleCrawlerPlugin
57
All plugin "stuff" is nearly the same as before. You additionally need to connect plugin to the octomap plugin output. To simplify this task you can use the "plugin holder". This template (parametrized by plugin type and octomap plugin type) contains plugin and do all signal-slot connections as defined by flags set in constructor. Holder declaration:
1 /// Declare holder object - partial specialization of the default holder with predefined connection settings
2 template< class tpOctomapPlugin >
3 struct SExampleCrawlerPluginHolder : public srs_env_model::CCrawlingPluginHolder< srs_env_model::CExampleCrawlerPlugin, tpOctomapPlugin >
4 {
5 protected:
6 /// Define holder type
7 typedef srs_env_model::CCrawlingPluginHolder< srs_env_model::CExampleCrawlerPlugin, tpOctomapPlugin > tHolder;
8
9 public:
10 /// Create holder and set what signals we need to connect
11 SExampleCrawlerPluginHolder( const std::string & name )
12 : tHolder( name, tHolder::ON_START | tHolder::ON_OCCUPIED |
13 tHolder::ON_FREE | tHolder::ON_STOP)
14 {
15
16 }
17
18 }; // struct SExampleCrawlerPluginHolder
19
Holder template final specialization variable in server class:
Instead of adding simple plugin to the plugins vector you need to get plugin from holder:
1 m_plugins.push_back( m_plugExampleCrawlerHolder.getPlugin() );
In publishAll server method connect plugin before crawl and disconnect it after:
And use following line instead of should publish - publish combo:
1 m_plugExampleCrawlerHolder.publish(rostime);
That's all...
