server_tools.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 SERVER_TOOLS_H_INCLUDED
#define SERVER_TOOLS_H_INCLUDED

#include <srs_env_model/but_server/octonode.h>

#include <boost/signal.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/make_shared.hpp>

// Small double number
#define SMALL_DOUBLE double(0.00000001);

template <typename tpType> tpType abs( tpType x ) { return x < 0.0 ? -x : x; }

template <typename tpType> bool isSmall( tpType x ) { return abs( x ) < SMALL_DOUBLE; }

template <typename tpType> bool isGreat( tpType x ) { return abs( x ) > SMALL_DOUBLE; }

namespace srs_env_model
{
        typedef octomap::ButOctomapROS< srs_env_model::EMOcTree > tButServerOcMap;

        typedef boost::shared_ptr<tButServerOcMap> tButServerOcMapPtr;

        typedef boost::shared_ptr< const tButServerOcMap > tButServerOcMapConstPtr;

        typedef tButServerOcMap::OcTreeType tButServerOcTree;

        typedef tButServerOcTree::NodeType tButServerOcNode;

        typedef pcl::PointXYZRGB tPclPoint;

        typedef pcl::PointCloud<tPclPoint> tPointCloud;

        typedef tPointCloud::Ptr tPointCloudPtr;

        typedef tPointCloud::ConstPtr tPointCloudConstPtr;

        struct SMapWithParameters
        {
        public:
                double resolution;

                unsigned treeDepth;

                unsigned char crawlDepth;

                double probHit;

                double probMiss;

                double thresMin;

                double thresMax;

                double thresOccupancy;

                double maxRange;

                unsigned mapSize;

                ros::Time currentTime;

                std::string frameId;

                boost::shared_ptr<tButServerOcMap> map;

        }; // struct SMapParameters.


        class CServerPluginBase
        {
        #define PERROR( x ) std::cerr << "Plugin "<< this->m_name << ": " << x << std::endl;

        public:
                CServerPluginBase( const std::string & name )
                        : m_name(name)
                { }

                virtual ~CServerPluginBase() {}

                virtual void init(ros::NodeHandle & node_handle)
                {
                }

                void publish(const ros::Time & timestamp)
                {
                    if( shouldPublish() ) publishInternal( timestamp );
                }

                virtual void reset() {}

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

                std::string getName( ) { return m_name; }


        protected:
                virtual bool shouldPublish() = 0;

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

        protected:
                std::string m_name;

                boost::mutex m_lockMutex;
        };


        template< class tpNodeType >
        class COctomapCrawlerBase
        {
        public:
                COctomapCrawlerBase() : m_crawlDepth(0) {}

                virtual ~COctomapCrawlerBase() {}

                void handleNewMapData( SMapWithParameters & par )
                {
                    if( wantsMap() ) newMapDataCB( par );
                }

                virtual bool wantsMap() { return true; }

        protected:
                virtual void newMapDataCB( SMapWithParameters & par ) = 0;

        protected:
                std::string m_frame_id;

                ros::Time m_time_stamp;

                unsigned char m_crawlDepth;
        };


        template< class tpDataType >
        class CDataHolderBase
        {
        public:
                typedef tpDataType tData;

                typedef boost::shared_ptr< tData > tDataPtr;

                typedef boost::shared_ptr< const tData > tDataConstPtr;

                typedef boost::signal< void (tDataConstPtr, const ros::Time & ) > tSigDataHasChanged;

                // 2012/12/14 Majkl: Trying to solve problem with missing time stamps
//        CDataHolderBase() : m_data(new tData) {}
//        CDataHolderBase() : m_data(new tData), m_DataTimeStamp(ros::Time(0)) {}
        CDataHolderBase() : m_DataTimeStamp(ros::Time::now()) {m_data = boost::make_shared<tData>();}

//        CDataHolderBase( tData * data ) : m_data(data) {}
//        CDataHolderBase( tData * data ) : m_data(data), m_DataTimeStamp(ros::Time(0)) {}
        CDataHolderBase( tData * data ) : m_data(data), m_DataTimeStamp(ros::Time::now()) {}

        public:
                virtual ~CDataHolderBase() {}

                tData & getData(){ return *m_data; }

                const tData & getData() const { return *m_data; }

                tSigDataHasChanged & getSigDataChanged() { return m_sigDataChanged; }

                virtual bool hasValidData() { return m_data != 0; }

                void invalidate()
                {
                        if( hasValidData() )
                        {
        //                      std::cerr << "invalidate: Locked." << std::endl;
                                boost::mutex::scoped_lock lock(m_lockData);
                                m_sigDataChanged( m_data, m_DataTimeStamp );
        //                      std::cerr << "invalidate: Unlocked." << std::endl;
                        }
                }

        protected:
                tDataPtr m_data;

                ros::Time m_DataTimeStamp;

                tSigDataHasChanged m_sigDataChanged;

                boost::mutex m_lockData;
        };

} // namespace srs_env_model

/*
POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointXYZRGB,
                (float, x, x)
                (float, y, y)
                (float, z, z)
                (uint32_t, rgb, rgb)
)
*/
// SERVER_TOOLS_H_INCLUDED
#endif