[Documentation] [TitleIndex] [WordIndex

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

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:

   1 #include <srs_env_model/but_server/plugins/example_plugin.h>
   2 .
   3 .
   4 .
   5 
   6 /// Add Example plugin variable to the srs_env_model::CButServer class
   7 protected:
   8    /// Create example plugins
   9    srs_env_model::CExamplePlugin m_plugExample;

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:

   1 , m_plugExample("Example plugin")
   2 .
   3 .
   4 .
   5 m_plugins.push_back( & m_plugExample );

Now add publishing to the server's publishAll() method:

   1 // Publish data
   2 if( m_plugExample.shouldPublish() )
   3   m_plugExample.onPublish(rostime);

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:

   1 /// Create crawler plugin holder
   2     srs_env_model::SExampleCrawlerPluginHolder< srs_env_model::COctoMapPlugin > m_plugExampleCrawlerHolder;

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:

   1 m_plugExampleCrawlerHolder.connect( & m_plugOctoMap );
   2 
   3 // Crawl octomap
   4 m_plugOctoMap.crawl( rostime );
   5 
   6 // Disconnect all
   7 m_plugExampleCrawlerHolder.disconnect();

And use following line instead of should publish - publish combo:

   1 m_plugExampleCrawlerHolder.publish(rostime);

That's all...


2019-10-12 13:09