cmap_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 CMapPubPlugin_H_included
#define CMapPubPlugin_H_included

#include <srs_env_model/but_server/server_tools.h>
#include <srs_env_model/GetCollisionMap.h>
#include <srs_env_model/IsNewCollisionMap.h>
#include <srs_env_model/LockCollisionMap.h>

#include <arm_navigation_msgs/CollisionMap.h>
#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <srs_env_model/RemoveCube.h>

namespace srs_env_model
{

    class CCMapPlugin : public CServerPluginBase, public COctomapCrawlerBase<tButServerOcTree::NodeType>, public CDataHolderBase< arm_navigation_msgs::CollisionMap >
    {
    protected:
        typedef COctomapCrawlerBase<tButServerOcTree::NodeType> tOctomapCrawler;

        typedef arm_navigation_msgs::CollisionMap::_boxes_type tBoxVec;

        typedef arm_navigation_msgs::OrientedBoundingBox tBox;

        typedef geometry_msgs::Point32 tBoxPoint;

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

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

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

        bool lockChanges( bool bLock );

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


    protected:
                virtual void newMapDataCB( SMapWithParameters & par );

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

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

        virtual bool shouldPublish(  );

        bool sameCMaps( arm_navigation_msgs::CollisionMap * map1, arm_navigation_msgs::CollisionMap * map2 );

        bool isNearRobot( const btVector3 & point, double extent );

        bool getCollisionMapSrvCallback( srs_env_model::GetCollisionMap::Request & req, srs_env_model::GetCollisionMap::Response & res );

        bool isNewCmapSrvCallback( srs_env_model::IsNewCollisionMap::Request & req, srs_env_model::IsNewCollisionMap::Response & res );

        bool lockCmapSrvCallback( srs_env_model::LockCollisionMap::Request & req, srs_env_model::LockCollisionMap::Response & res );

        long removeInsideBox( const tBoxPoint & center, const tBoxPoint & size, tBoxVec & boxes );

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

        void addBox( const tBoxPoint & center, const tBoxPoint & size, tBoxVec & boxes );

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

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

    protected:
        std::string m_cmapPublisherName;

        ros::Publisher m_cmapPublisher;

        double m_collisionMapLimitRadius;

        long int m_collisionMapVersion;

        tf::Stamped<btVector3> m_robotBasePosition;

        std::string m_cmapFrameId;

        tDataPtr m_dataBuffer;

        arm_navigation_msgs::CollisionMap m_dataEmpty;

        bool m_publishCollisionMap;

        ros::ServiceServer m_serviceGetCollisionMap;

        ros::ServiceServer m_serviceIsNewCMap;

        ros::ServiceServer m_serviceLockCMap;

        ros::ServiceServer m_serviceRemoveCube;

        ros::ServiceServer m_serviceAddCube;

        tf::TransformListener m_tfListener;

        Eigen::Matrix4f m_worldToCMapTM;

        Eigen::Matrix3f m_worldToCMapRot;
        Eigen::Vector3f m_worldToCMapTrans;

        //
        bool m_latchedTopics;

        bool m_bConvertPoint;

        bool m_bLocked;

        ros::Time m_mapTime;


    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    }; // class CCollisionMapPublisher



}

// CCMapPubPlugin_H_included
#endif