limited_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 LimitedPointCloudPlugin_H_included
#define LimitedPointCloudPlugin_H_included

#include "point_cloud_plugin.h"

#include <srs_env_model_msgs/RVIZCameraPosition.h>
#include <srs_env_model/but_server/server_tools.h>

#include <boost/scoped_ptr.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/recursive_mutex.hpp>


namespace srs_env_model
{
class CLimitedPointCloudPlugin : public CPointCloudPlugin
{
public:
        CLimitedPointCloudPlugin( const std::string & name );

        virtual ~CLimitedPointCloudPlugin();

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

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

        virtual bool wantsMap() { return m_cameraFrameId.size() != 0; }

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
        virtual void newMapDataCB( SMapWithParameters & par );

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

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

        void onCameraPositionChangedCB(const srs_env_model_msgs::RVIZCameraPosition::ConstPtr& position);

        // main loop when spinning our own thread
        // - process callbacks in our callback queue
        // - process pending goals
        void spinThread();

protected:
        bool m_bTransformCamera;

        std::string m_cameraFrameId;

    // Camera position topic name
    std::string m_cameraPositionTopic;

        Eigen::Vector3f m_normal, m_normalBuf;

        float m_d, m_dBuf;

        Eigen::Matrix3f m_camToOcRot;

        Eigen::Vector3f m_camToOcTrans;

        // message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition> *m_camPosSubscriber;
        ros::Subscriber m_camPosSubscriber;

        tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition> *m_tfCamPosSub;

        long m_countVisible, m_countHidden;

        float min, max;

        bool m_bSpinThread;

        // these are needed when spinning up a dedicated thread
        boost::scoped_ptr<boost::thread> spin_thread_;
        ros::NodeHandle node_handle_;
        ros::CallbackQueue callback_queue_;
        volatile bool need_to_terminate_;

        // Mutex used to lock camera position parameters
        boost::recursive_mutex m_camPosMutex;
};

} // namespace srs_env_model

// LimitedPointCloudPlugin_H_included
#endif