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 CompressedPointCloudPlugin_H_included 00029 #define CompressedPointCloudPlugin_H_included 00030 00031 #include "point_cloud_plugin.h" 00032 00033 #include <srs_env_model_msgs/RVIZCameraPosition.h> 00034 #include <srs_env_model/but_server/server_tools.h> 00035 #include <srs_env_model/OctomapUpdates.h> 00036 #include <srs_env_model/SetNumIncompleteFrames.h> 00037 00038 #include <boost/scoped_ptr.hpp> 00039 #include <boost/thread/thread.hpp> 00040 #include <boost/thread/recursive_mutex.hpp> 00041 #include <image_geometry/pinhole_camera_model.h> 00042 00043 00044 namespace srs_env_model 00045 { 00046 class CCompressedPointCloudPlugin : public CPointCloudPlugin 00047 { 00048 public: 00050 CCompressedPointCloudPlugin( const std::string & name ); 00051 00053 virtual ~CCompressedPointCloudPlugin(); 00054 00056 virtual void init(ros::NodeHandle & node_handle); 00057 00059 virtual void pause( bool bPause, ros::NodeHandle & node_handle); 00060 00061 00062 public: 00063 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00064 00065 protected: 00067 virtual void newMapDataCB( SMapWithParameters & par ); 00068 00070 virtual void handleOccupiedNode(tButServerOcTree::iterator& it, const SMapWithParameters & mp); 00071 00073 virtual bool shouldPublish(); 00074 00076 void publishInternal(const ros::Time & timestamp); 00077 00079 void onCameraChangedCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info); 00080 00081 // main loop when spinning our own thread 00082 // - process callbacks in our callback queue 00083 // - process pending goals 00084 void spinThread(); 00085 00087 bool inSensorCone(const cv::Point2d& uv) const; 00088 00090 bool setNumIncompleteFramesCB( srs_env_model::SetNumIncompleteFrames::Request & req, srs_env_model::SetNumIncompleteFrames::Response & res ); 00091 00092 protected: 00094 bool m_bTransformCamera; 00095 00097 std::string m_cameraFrameId; 00098 00099 // Camera position topic name 00100 std::string m_cameraInfoTopic; 00101 00102 00104 // message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition> *m_camPosSubscriber; 00105 ros::Subscriber m_camPosSubscriber; 00106 00108 ros::Publisher m_ocUpdatePublisher; 00109 00111 std::string m_ocUpdatePublisherName; 00112 00114 tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition> *m_tfCamPosSub; 00115 00117 long m_countVisible, m_countAll; 00118 00119 float min, max; 00120 00122 bool m_bSpinThread; 00123 00124 // these are needed when spinning up a dedicated thread 00125 boost::scoped_ptr<boost::thread> spin_thread_; 00126 ros::NodeHandle node_handle_; 00127 ros::CallbackQueue callback_queue_; 00128 volatile bool need_to_terminate_; 00129 00130 // Mutex used to lock camera position parameters 00131 boost::recursive_mutex m_camPosMutex; 00132 00134 image_geometry::PinholeCameraModel m_camera_model, m_camera_model_buffer; 00135 00137 tf::TransformListener m_tfListener; 00138 00140 tf::Transform m_to_sensor; 00141 00143 int m_camera_stereo_offset_left, m_camera_stereo_offset_right; 00144 00146 cv::Size m_camera_size, m_camera_size_buffer; 00147 00149 sensor_msgs::CameraInfo m_camera_info_buffer; 00150 00152 bool m_bCamModelInitialized; 00153 00155 long m_frame_counter; 00156 00158 long m_uncomplete_frames; 00159 00161 bool m_bPublishComplete; 00162 00164 bool m_bCreatePackedInfoMsg; 00165 00167 bool m_bPublishSimpleCloud; 00168 00170 srs_env_model::OctomapUpdatesPtr m_octomap_updates_msg; 00171 00173 ros::ServiceServer m_serviceSetNumIncomplete; 00174 00176 bool m_bTransformOutput; 00177 }; 00178 00179 } // namespace srs_env_model 00180 00181 // CompressedPointCloudPlugin_H_included 00182 #endif 00183