00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 #pragma once 00028 #ifndef CollisionGridPlugin_H_included 00029 #define CollisionGridPlugin_H_included 00030 00031 #include <srs_env_model/but_server/server_tools.h> 00032 00033 #include <nav_msgs/OccupancyGrid.h> 00034 #include <message_filters/subscriber.h> 00035 #include <tf/message_filter.h> 00036 00037 namespace srs_env_model 00038 { 00039 00040 class CCollisionGridPlugin : public CServerPluginBase, public COctomapCrawlerBase<tButServerOcTree::NodeType>, public CDataHolderBase< nav_msgs::OccupancyGrid > 00041 { 00042 public: 00044 CCollisionGridPlugin(const std::string & name); 00045 00047 virtual ~CCollisionGridPlugin(); 00048 00050 void enable( bool enabled ){ m_publishGrid = enabled; } 00051 00053 virtual void init(ros::NodeHandle & node_handle); 00054 00056 virtual void pause( bool bPause, ros::NodeHandle & node_handle ); 00057 00058 protected: 00060 bool shouldPublish(); 00061 00063 virtual void publishInternal(const ros::Time & timestamp); 00064 00066 virtual void newMapDataCB( SMapWithParameters & par ); 00067 00069 virtual void handleFreeNode(tButServerOcTree::iterator & it, const SMapWithParameters & mp ); 00070 00072 virtual void handleOccupiedNode(tButServerOcTree::iterator& it, const SMapWithParameters & mp); 00073 00074 public: 00075 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00076 00077 protected: 00079 bool m_publishGrid; 00080 00082 std::string m_gridPublisherName; 00083 00085 ros::Publisher m_gridPublisher; 00086 00088 tf::TransformListener m_tfListener; 00089 00090 // 00091 bool m_latchedTopics; 00092 00094 std::string m_ocFrameId; 00095 00097 Eigen::Matrix3f m_ocToGridRot; 00098 00100 Eigen::Vector3f m_ocToGridTrans; 00101 00103 octomap::OcTreeKey m_paddedMinKey; 00104 00106 double m_minSizeX; 00107 double m_minSizeY; 00108 00110 bool m_bConvert; 00111 00113 unsigned m_multires2DScale; 00114 00115 }; // class CCollisionGridPlugin 00116 00117 00118 } // namespace srs_env_model 00119 00120 00121 00122 // namespace srs_env_model 00123 00124 00125 // CollisionGridPlugin_H_included 00126 #endif 00127