scan_unifier_node.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#ifndef SCAN_UNIFIER_NODE_H
#define SCAN_UNIFIER_NODE_H

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

// standard includes
#include <pthread.h>
#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>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

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


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

    config_struct config_;

    std::string frame_;

    std::vector<message_filters::Subscriber<sensor_msgs::LaserScan>* > message_filter_subscribers_;

    message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan,
                                                                                  sensor_msgs::LaserScan> >* synchronizer2_;
    message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan,
                                                                                  sensor_msgs::LaserScan,
                                                                                  sensor_msgs::LaserScan> >* synchronizer3_;
    message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan,
                                                                                  sensor_msgs::LaserScan,
                                                                                  sensor_msgs::LaserScan,
                                                                                  sensor_msgs::LaserScan> >* synchronizer4_;

    void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner,
                               const sensor_msgs::LaserScan::ConstPtr& second_scanner);
    void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner,
                               const sensor_msgs::LaserScan::ConstPtr& second_scanner,
                               const sensor_msgs::LaserScan::ConstPtr& third_scanner);
    void messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner,
                               const sensor_msgs::LaserScan::ConstPtr& second_scanner,
                               const sensor_msgs::LaserScan::ConstPtr& third_scanner,
                               const sensor_msgs::LaserScan::ConstPtr& fourth_scanner);

  public:

    // constructor
    ScanUnifierNode();

    // destructor
    ~ScanUnifierNode();

    /* ----------------------------------- */
    /* --------- 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_;

    std::vector<sensor_msgs::PointCloud> vec_cloud_;

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

    void getParams();

    bool unifyLaserScans(const std::vector<sensor_msgs::LaserScan::ConstPtr>& current_scans,
                         sensor_msgs::LaserScan& unified_scan);
};
#endif