compressed_pc_publisher.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Vit Stancl (stancl@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: dd/mm/2012
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef COMPRESSED_PC_PUBLISHER_H_INCLUDED
#define COMPRESSED_PC_PUBLISHER_H_INCLUDED

#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>
#include <srs_env_model/OctomapUpdates.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>


namespace srs_env_model
{

class CCompressedPCPublisher
{
public:
        typedef pcl::PointXYZRGB tPclPoint;

        typedef pcl::PointCloud<tPclPoint> tPointCloudInternal;

        typedef srs_env_model::OctomapUpdates tInputData;

        typedef tInputData::_pointcloud2_type tInputCloudMsg;

        typedef sensor_msgs::PointCloud2 tOutputCloudMsg;

        typedef tInputData::_camera_info_type tCameraInfo;

public:
        CCompressedPCPublisher(ros::NodeHandle & nh);

protected:
        void incommingDataCB( const tInputData::ConstPtr & data );

        long copyCloud( const tInputCloudMsg & input, tPointCloudInternal & output, bool bAdd = false );

        long removeFrustumFromCloud( const tInputData::ConstPtr & data, tPointCloudInternal & cloud );

    bool isRGBCloud( const tInputCloudMsg & cloud );

    bool inSensorCone(const cv::Point2d& uv) const;

protected:
        std::string m_publishedTopicName;

        ros::Publisher m_pub;

        std::string m_subscribedTopicName;

//      ros::Subscriber m_sub;
        message_filters::Subscriber<tInputData> m_sub;
        tf::MessageFilter<tInputData> * m_tf_filter;

        tPointCloudInternal m_cloud;

        int m_camera_stereo_offset_left, m_camera_stereo_offset_right;

        cv::Size m_camera_size;

        std::string m_cameraFrameId;

        image_geometry::PinholeCameraModel m_camera_model;

        tf::Transform m_to_sensor;

        tf::Transform m_to_pfid;

        std::string m_publishFID, m_cameraFID;

        tf::TransformListener m_tfListener, m_tfListener2;

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};


} // namespace srs_env_model

// COMPRESSED_PC_PUBLISHER_H_INCLUDED
#endif