OctomapServerCombined.h
Go to the documentation of this file.00001 00009 /* 00010 * Copyright (c) 2010-2011, A. Hornung, University of Freiburg 00011 * All rights reserved. 00012 * 00013 * Redistribution and use in source and binary forms, with or without 00014 * modification, are permitted provided that the following conditions are met: 00015 * 00016 * * Redistributions of source code must retain the above copyright 00017 * notice, this list of conditions and the following disclaimer. 00018 * * Redistributions in binary form must reproduce the above copyright 00019 * notice, this list of conditions and the following disclaimer in the 00020 * documentation and/or other materials provided with the distribution. 00021 * * Neither the name of the University of Freiburg nor the names of its 00022 * contributors may be used to endorse or promote products derived from 00023 * this software without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00026 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00027 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00028 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00029 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00030 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00031 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00032 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00033 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00034 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 */ 00037 00038 00039 #include <ros/ros.h> 00040 #include <visualization_msgs/MarkerArray.h> 00041 #include <nav_msgs/OccupancyGrid.h> 00042 #include <std_msgs/ColorRGBA.h> 00043 #include <mapping_msgs/CollisionObject.h> 00044 #include <sensor_msgs/PointCloud2.h> 00045 00046 #include <pcl/point_types.h> 00047 #include <pcl/ros/conversions.h> 00048 #include <pcl_ros/transforms.h> 00049 #include <pcl/sample_consensus/method_types.h> 00050 #include <pcl/sample_consensus/model_types.h> 00051 #include <pcl/segmentation/sac_segmentation.h> 00052 #include <pcl/io/pcd_io.h> 00053 #include <pcl/filters/extract_indices.h> 00054 #include <pcl/filters/passthrough.h> 00055 00056 00057 #include <tf/transform_listener.h> 00058 #include <tf/message_filter.h> 00059 #include <message_filters/subscriber.h> 00060 #include <octomap_ros/OctomapBinary.h> 00061 #include <octomap_ros/GetOctomap.h> 00062 #include <octomap_ros/OctomapROS.h> 00063 #include <octomap/OcTreeKey.h> 00064 00065 00066 namespace octomap { 00067 class OctomapServerCombined{ 00068 public: 00069 OctomapServerCombined(const std::string& filename= ""); 00070 virtual ~OctomapServerCombined(); 00071 bool serviceCallback(octomap_ros::GetOctomap::Request &req, 00072 octomap_ros::GetOctomap::Response &res); 00073 00074 void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud); 00075 00076 private: 00077 std_msgs::ColorRGBA heightMapColor(double h) const; 00078 void publishMap(const ros::Time& rostime = ros::Time::now()); 00079 void publishAll(const ros::Time& rostime = ros::Time::now()); 00080 ros::NodeHandle m_nh; 00081 ros::Publisher m_markerPub, m_binaryMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub; 00082 message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub; 00083 tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub; 00084 ros::ServiceServer m_service; 00085 tf::TransformListener m_tfListener; 00086 00087 OcTreeROS m_octoMap; 00088 KeyRay m_keyRay; // temp storage for ray casting 00089 double m_maxRange; 00090 std::string m_worldFrameId; // the map frame 00091 std::string m_baseFrameId; // base of the robot for ground plane filtering 00092 bool m_useHeightMap; 00093 std_msgs::ColorRGBA m_color; 00094 double m_colorFactor; 00095 00096 double m_pointcloudMinZ; 00097 double m_pointcloudMaxZ; 00098 double m_occupancyMinZ; 00099 double m_occupancyMaxZ; 00100 double m_minSizeX; 00101 double m_minSizeY; 00102 bool m_filterSpeckles; 00103 00104 bool m_filterGroundPlane; 00105 double m_groundFilterDistance; 00106 double m_groundFilterAngle; 00107 double m_groundFilterPlaneDistance; 00108 }; 00109 } 00110