nucManager.hpp

Go to the documentation of this file.

/* This class implements a simple NUC manager that allows or forbids NUC depending on the current camera rotation.
 * The parameter rotThreshold can be changed in the launch file and the default value can also be changed in the constructor.
 * 
 * The node subscribes to a "pose" message, analyses the current amount of rotation and then publishes a topic called nuc_flag, which contains a data field with valeu 0 or 1 (1: NUC allowed, 0: NUC forbidden)
*/


#include <vector>
#include <string>
#include <ros/node_handle.h>
#include <ros/init.h>

#include <ros/ros.h>
#include <std_msgs/Float32.h>

#include <geometry_msgs/PoseStamped.h>
//#include <geometry_messages/Twist.h>


#ifndef _THERMALVIS_NUCMANAGER_H_
#define _THERMALVIS_NUCMANAGER_H_

 
const char __PROGRAM__[] = "THERMALVIS_NUCMANAGER";



class nucManager{
  
  private:

  std::vector<float> quadBuffer;
  
  ros::NodeHandle nh_;

  ros::Subscriber pose_sub_;
  ros::Publisher nucFlag_pub_;
  
  std_msgs::Float32 nuc_msg;
  geometry_msgs::PoseStamped poseStp;
  
  // Input parameters 
  double rotThreshold;      
  std::string inputTopic;
    
  
  public:
    nucManager()
    {
      
      ros::NodeHandle nh_read("~");
      
      nh_read.param("rotThreshold", rotThreshold, 0.004);
      nh_read.param<std::string>("inputTopic", inputTopic, "mono_odometer/pose");
      
      nucFlag_pub_  = nh_.advertise<std_msgs::Float32>("thermalvis/nuc_flag", 1);
      pose_sub_ = nh_.subscribe(inputTopic, 1, &nucManager::poseCallBack, this);
     
    }

  
    void poseCallBack(const geometry_msgs::PoseStamped msg)
    {
      
      poseStp = msg;
      
      quadBuffer.push_back(msg.pose.orientation.y);
      
      if(quadBuffer.size() > 2)
      {
        quadBuffer.erase(quadBuffer.begin() );
        
      }
      
    }
    
    //Function to evaluate the amount of rotation and set the NUC permission flag
    void setFlag();
    
    // Simply gives access to the current flag status
    float getStatus();
    
    // Publishes the message
    void publish()
    {
      nucFlag_pub_.publish(nuc_msg);
    };
    

      
   
  
};



#endif