octomap_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 OCTOMAPPLUGIN_H_INCLUDED
#define OCTOMAPPLUGIN_H_INCLUDED

#include <srs_env_model/but_server/server_tools.h>
#include <srs_env_model/but_server/plugins/octomap_plugin_tools/testing_oriented_box.h>
#include <srs_env_model/but_server/plugins/octomap_plugin_tools/testing_sphere.h>
#include <srs_env_model/but_server/plugins/octomap_plugin_tools/testing_polymesh.h>

#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>

#include <srs_env_model/RemoveCube.h>
#include <srs_env_model/AddCube.h>
#include <srs_env_model/SetCrawlingDepth.h>
#include <srs_env_model/GetTreeDepth.h>

#include <srs_env_model/LoadSave.h>

// Registration
#include <srs_env_model/but_server/registration/CPCtoOCRegistration.h>

#include "octomap_plugin_tools/octomap_filter_single_specles.h"
#include "octomap_plugin_tools/octomap_filter_raycast.h"
#include "octomap_plugin_tools/octomap_filter_ground.h"

//========================
// Filtering

#include <image_geometry/pinhole_camera_model.h>

namespace srs_env_model
{
class CPointCloudPlugin;

class COctoMapPlugin : public CServerPluginBase, public CDataHolderBase< tButServerOcMap >
{
public:
        typedef boost::signal< void (SMapWithParameters &) > tSigOnNewData;

public:
        COctoMapPlugin(const std::string & name);

        COctoMapPlugin( const std::string & name, const std::string & filename );

        virtual ~COctoMapPlugin();

        void insertCloud(tPointCloud::ConstPtr cloud);

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

        void reset(bool clearLoaded = true);

        unsigned getSize() { return m_data->getTree().size(); }

        unsigned getTreeDepth() { return m_mapParameters.treeDepth; }

        double getResolution(){ return m_mapParameters.resolution; }

        void crawl( const ros::Time & currentTime );

        tSigOnNewData & getSigOnNewData() { return m_sigOnNewData; }

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

protected:
        virtual bool shouldPublish();

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

        void setDefaults();

        void insertScan(const tf::Point& sensorOriginTf, const tPointCloud& ground, const tPointCloud& nonground);

        void fillMapParameters(const ros::Time & time);

        bool resetOctomapCB(std_srvs::Empty::Request& request,  std_srvs::Empty::Response& response);

        void filterCloud( tPointCloudConstPtr & cloud);


        // ------------------------------------------------------------------------
        // Obstacle cleaning

        long int doObjectTesting( CTestingObjectBase * object );

        bool removeCubeCB( srs_env_model::RemoveCube::Request & req, srs_env_model::RemoveCube::Response & res );

        bool addCubeCB( srs_env_model::AddCube::Request & req, srs_env_model::AddCube::Response & res );

        void addCubeGizmo( const geometry_msgs::Pose & pose, const geometry_msgs::Point & size );

        bool setCrawlingDepthCB( srs_env_model::SetCrawlingDepth::Request & req, srs_env_model::SetCrawlingDepth::Response & res );

        bool getTreeDepthCB( srs_env_model::GetTreeDepth::Request & req, srs_env_model::GetTreeDepth::Response & res );

        bool loadOctreeCB( srs_env_model::LoadSaveRequest & req, srs_env_model::LoadSaveResponse & res );

        bool saveOctreeCB( srs_env_model::LoadSaveRequest & req, srs_env_model::LoadSaveResponse & res );

        bool loadFullOctreeCB( srs_env_model::LoadSaveRequest & req, srs_env_model::LoadSaveResponse & res );

        bool saveFullOctreeCB( srs_env_model::LoadSaveRequest & req, srs_env_model::LoadSaveResponse & res );


protected:

    bool m_filterGroundPlane;

    octomap::KeyRay m_keyRay;

    tSigOnNewData m_sigOnNewData;

    SMapWithParameters m_mapParameters;

    tf::TransformListener m_tfListener;

    ros::ServiceServer m_serviceResetOctomap;

    ros::ServiceServer m_serviceRemoveCube;

    ros::ServiceServer m_serviceAddCube;

    ros::ServiceServer m_serviceSetCrawlDepth;

    ros::ServiceServer m_serviceGetTreeDepth;

    ros::ServiceServer m_serviceLoadMap;

    ros::ServiceServer m_serviceSaveMap;

        ros::ServiceServer m_serviceLoadFullMap;

        ros::ServiceServer m_serviceSaveFullMap;

    bool m_bPublishOctomap;

    std::string m_ocPublisherName;

    ros::Publisher m_ocPublisher;

    bool m_latchedTopics;

    bool m_removeSpecles;

    int filecounter;

    //=========================================================================
    // Filtering

    COcFilterSingleSpecles m_filterSingleSpecles;
    COcFilterRaycast m_filterRaycast;
    COcFilterGround m_filterGround;

    bool m_bFilterWithInput;

    boost::shared_ptr< CPointCloudPlugin > m_filterCloudPlugin;

    bool m_bRemoveOutdated;

    bool m_bNewDataToFilter;

    CTestingObjectBase * m_removeTester;

    unsigned int m_testerLife;

    unsigned int m_testerLifeCounter;

    unsigned char m_crawlDepth;

    CPcToOcRegistration m_registration;

    bool m_bNotFirst;

    bool m_bMapLoaded;

    float m_probDeleted;

        uint8_t m_r, m_g, m_b;


}; // class COctoMapPlugin;


}



 // namespace srs_env_model

// OCTOMAPPLUGIN_H_INCLUDED
#endif