scan_unifier_node.h

Go to the documentation of this file.

#ifndef SCAN_UNIFIER_NODE_H
#define SCAN_UNIFIER_NODE_H

//##################
//#### includes ####

// standard includes
#include <pthread.h>
#include <boost/lexical_cast.hpp>
#include <XmlRpc.h>
#include <math.h>

// ROS includes
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <sensor_msgs/PointCloud.h>
#include <laser_geometry/laser_geometry.h>

// ROS message includes
#include <sensor_msgs/LaserScan.h>


//####################
//#### node class ####
class scan_unifier_node
{
  private:
    struct config_struct{
      int number_input_scans;
      double loop_rate;
      std::vector<std::string> input_scan_topics;
    };

    struct laser_scan_struct{
      bool new_msg_received;
      int scan_id;
      std::string scan_topic;
      ros::Subscriber laser_sub;
      sensor_msgs::LaserScan current_scan_msg;
    };

    pthread_mutex_t m_mutex;

    config_struct config_;

    std::vector<laser_scan_struct> vec_laser_struct_;
  public:

    // constructor
    scan_unifier_node();

    // destructor
    ~scan_unifier_node();

    /* ----------------------------------- */
    /* --------- ROS Variables ----------- */
    /* ----------------------------------- */

    // create node handles
    ros::NodeHandle nh_, pnh_;

    // declaration of ros publishers
    ros::Publisher topicPub_LaserUnified_;

    // tf listener
    tf::TransformListener listener_;

    // laser geometry projector
    laser_geometry::LaserProjection projector_;

    /* ----------------------------------- */
    /* ----------- functions ------------- */
    /* ----------------------------------- */

    void getParams();

    void set_new_msg_received(bool received, int scan_id);

    bool get_new_msg_received(int scan_id);

    double getLoopRate();

    void initLaserScanStructs();

    void topicCallbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan_in, int scan_id);

    void checkUnifieCondition();

    sensor_msgs::LaserScan unifieLaserScans();

};
#endif