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 CollisionObjectPlugin_H_included 00029 #define CollisionObjectPlugin_H_included 00030 00031 #include <srs_env_model/but_server/server_tools.h> 00032 00033 #include <message_filters/subscriber.h> 00034 #include <arm_navigation_msgs/CollisionObject.h> 00035 #include <tf/message_filter.h> 00036 00037 namespace srs_env_model 00038 { 00039 00040 class CCollisionObjectPlugin : public CServerPluginBase, public COctomapCrawlerBase<tButServerOcTree::NodeType>, public CDataHolderBase< arm_navigation_msgs::CollisionObject > 00041 { 00042 public: 00044 CCollisionObjectPlugin(const std::string & name); 00045 00047 virtual ~CCollisionObjectPlugin(); 00048 00050 void enable( bool enabled ){ m_publishCollisionObject = enabled; } 00051 00053 bool shouldPublish(); 00054 00056 virtual void init(ros::NodeHandle & node_handle); 00057 00059 virtual void pause( bool bPause, ros::NodeHandle & node_handle); 00060 00061 protected: 00063 virtual void publishInternal(const ros::Time & timestamp); 00064 00066 virtual void newMapDataCB( SMapWithParameters & par ); 00067 00069 virtual void handleOccupiedNode(tButServerOcTree::iterator& it, const SMapWithParameters & mp); 00070 00071 public: 00072 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00073 00074 protected: 00076 bool m_publishCollisionObject; 00077 00079 std::string m_coPublisherName; 00080 00082 ros::Publisher m_coPublisher; 00083 00085 tf::TransformListener m_tfListener; 00086 00087 // 00088 bool m_latchedTopics; 00089 00091 std::string m_coFrameId; 00092 00094 std::string m_ocFrameId; 00095 00097 Eigen::Matrix3f m_ocToCoRot; 00098 00100 Eigen::Vector3f m_ocToCoTrans; 00101 00103 bool m_bConvert; 00104 00105 }; // class CCollisionObjectPlugin 00106 00107 } 00108 00109 00110 00111 // namespace srs_env_model 00112 00113 00114 // CollisionObjectPlugin_H_included 00115 #endif 00116