Add diagnostic: module state

include/iri_nav_module/nav_module.h

@@ -22,6 +22,8 @@
 
 #include <iri_nav_module/nav_costmap_module.h>
 
+#include <diagnostic_updater/diagnostic_updater.h>
+
 /**
   * \brief Navigation module
   *
@@ -205,17 +207,42 @@ class CNavModule : public CModule<ModuleCfg>
       * \brief
       *
       */
-    virtual void state_machine(void)=0;
+    virtual void state_machine(void){
+      // update diagnostics
+      this->diagnostic_updater_.update();
+      ros::WallDuration(1.0).sleep(); //TODO: use cfg param with update rate
+      
+    };
     /**
       * \brief
       *
       */
     virtual void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
+    
+    /**
+      * \brief
+      *
+      */
+    void module_running_state(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    
+    /**
+      * \brief
+      *
+      */
+    diagnostic_updater::FunctionDiagnosticTask module_running_state_diagnostic;
+    
     /**
       * \brief
       *
       */
     bool transform_goal(double &x,double &y,double &yaw);
+    /**
+    * \brief diagnostic updater
+    * 
+    * The diagnostic updater allows definition of custom diagnostics. 
+    * 
+    */
+    diagnostic_updater::Updater diagnostic_updater_;
 
   public:
     /**
@@ -223,6 +250,7 @@ class CNavModule : public CModule<ModuleCfg>
       *
       */
     CNavModule(const std::string &name,const std::string &name_space=std::string(""));
+
     /**
       * \brief
       *
@@ -413,6 +441,13 @@ class CNavModule : public CModule<ModuleCfg>
      *
      */
     const CNavCostmapModule<ModuleCfg> &get_global_costmap(void) const;
+    
+    /**
+     * \brief
+     *
+     */
+    void check_module_running_state(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    
     /**
       * \brief Destructor
       *
@@ -423,12 +458,14 @@ class CNavModule : public CModule<ModuleCfg>
   template <class ModuleCfg>
   CNavModule<ModuleCfg>::CNavModule(const std::string &name,const std::string &name_space) : CModule<ModuleCfg>(name,name_space),
     tf2_listener(tf2_buffer),
+    diagnostic_updater_(),
     move_base_action("move_base",this->module_nh.getNamespace()),
     move_base_reconf("move_base_reconf",this->module_nh.getNamespace()),
     make_plan_service("make_plan",this->module_nh.getNamespace()),
     clear_costmaps("clear_costmaps",this->module_nh.getNamespace()),
     local_costmap("local_costmap",this->module_nh.getNamespace()),
-    global_costmap("global_costmap",this->module_nh.getNamespace())
+    global_costmap("global_costmap",this->module_nh.getNamespace()),
+    module_running_state_diagnostic(this->module_nh.getNamespace()+"/module_running_state",boost::bind(&CNavModule<ModuleCfg>::check_module_running_state, this, _1))
   {
     //Variables
     this->path_available=false;
@@ -451,6 +488,9 @@ class CNavModule : public CModule<ModuleCfg>
     this->robot_frame_id="base_link";
     this->global_frame_id="map";
     this->goal_frame_id="map";
+    
+    //diagnostic_updater::FunctionDiagnosticTask module_running_state_diagnostic("module_running_state",boost::bind(&CNavModule<ModuleCfg>::check_module_running_state, this));
+    this->diagnostic_updater_.add(this->module_running_state_diagnostic);
   }
 
   template <class ModuleCfg>
@@ -1070,6 +1110,15 @@ class CNavModule : public CModule<ModuleCfg>
   {
     return this->global_costmap;
   }
+  
+  template <class ModuleCfg>
+  void CNavModule<ModuleCfg>::check_module_running_state(diagnostic_updater::DiagnosticStatusWrapper &stat)
+  {
+    if(this->move_base_action.get_state()==ACTION_RUNNING)
+     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "running");
+    else
+     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "not running");
+  }
 
   template <class ModuleCfg>
   CNavModule<ModuleCfg>::~CNavModule()