point_cloud_plugin.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 PointCloudPlugin_H_included
#define PointCloudPlugin_H_included

#include <srs_env_model/but_server/server_tools.h>

#include <message_filters/subscriber.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/message_filter.h>

#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/pass_through.h>

// PCL includes
#include <pcl/segmentation/extract_polygonal_prism_data.h>


namespace srs_env_model
{

    class CPointCloudPlugin : public CServerPluginBase, public COctomapCrawlerBase<tButServerOcTree::NodeType>, public CDataHolderBase< tPointCloud >
    {
    public:
        typedef sensor_msgs::PointCloud2 tIncommingPointCloud;

    public:
        CPointCloudPlugin(const std::string & name, bool subscribe );

        virtual ~CPointCloudPlugin();

        void enable( bool enabled ) { m_publishPointCloud = enabled; }

        virtual void init(ros::NodeHandle & node_handle);

        virtual void init(ros::NodeHandle & node_handle, bool subscribe){ m_bSubscribe = subscribe; init(node_handle); }

        virtual void init(ros::NodeHandle & node_handle, const std::string & topic){ m_pcSubscriberName = topic; init(node_handle); }

        virtual void pause( bool bPause, ros::NodeHandle & node_handle );

        virtual bool wantsMap();

                void setFrameSkip(unsigned long skip){ m_use_every_nth = skip; }

                void setUseInputColor(bool bUseInputColor) {boost::mutex::scoped_lock lock(m_lockData); m_bUseInputColor = bUseInputColor;}

                void setDefaultColor(uint8_t r, uint8_t g, uint8_t b ){ boost::mutex::scoped_lock lock(m_lockData); m_r = r; m_g = g; m_b = b; }

    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    protected:
      virtual void newMapDataCB( SMapWithParameters & par );

      virtual bool shouldPublish();

      virtual void publishInternal( const ros::Time & timestamp );

          void handleOccupiedNode(tButServerOcTree::iterator& it, const SMapWithParameters & mp);

        void insertCloudCallback(const tIncommingPointCloud::ConstPtr& cloud);

        bool isRGBCloud( const tIncommingPointCloud::ConstPtr& cloud );

                virtual bool useFrame() { return ++m_frame_number % m_use_every_nth == 0; }

    protected:
        bool m_publishPointCloud;

        std::string m_pcPublisherName;

        std::string m_pcSubscriberName;

        message_filters::Subscriber<tIncommingPointCloud> *m_pcSubscriber;

        tf::MessageFilter<tIncommingPointCloud> *m_tfPointCloudSub;

        ros::Publisher m_pcPublisher;

        std::string m_inputPcFrameId;

        bool m_bSubscribe;

        tf::TransformListener m_tfListener;

        //
        bool m_latchedTopics;

        bool m_bFilterPC;

        double m_pointcloudMinZ;

        double m_pointcloudMaxZ;

        long counter;

        bool m_bAsInput;

        Eigen::Matrix4f m_pcOutTM;

        tPointCloudPtr m_oldCloud;

        tPointCloudPtr m_bufferCloud;

                unsigned long m_frame_number;

                unsigned long m_use_every_nth;

            bool m_bUseInputColor;

            uint8_t m_r, m_g, m_b;


    }; // class CPointCloudPlugin


} // namespace srs_env_model


// PointCloudPubPlugin_H_included
#endif