Go to the documentation of this file.00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef OCTOMAP_SERVER_OCTOMAPSERVER_H
00039 #define OCTOMAP_SERVER_OCTOMAPSERVER_H
00040
00041 #include <ros/ros.h>
00042 #include <visualization_msgs/MarkerArray.h>
00043 #include <nav_msgs/OccupancyGrid.h>
00044 #include <std_msgs/ColorRGBA.h>
00045 #include <arm_navigation_msgs/CollisionObject.h>
00046 #include <arm_navigation_msgs/CollisionMap.h>
00047 #include <sensor_msgs/PointCloud2.h>
00048 #include <std_srvs/Empty.h>
00049 #include <dynamic_reconfigure/server.h>
00050 #include <octomap_server/OctomapServerConfig.h>
00051
00052 #include <pcl/point_types.h>
00053 #include <pcl/ros/conversions.h>
00054 #include <pcl_ros/transforms.h>
00055 #include <pcl/sample_consensus/method_types.h>
00056 #include <pcl/sample_consensus/model_types.h>
00057 #include <pcl/segmentation/sac_segmentation.h>
00058 #include <pcl/io/pcd_io.h>
00059 #include <pcl/filters/extract_indices.h>
00060 #include <pcl/filters/passthrough.h>
00061
00062
00063 #include <tf/transform_listener.h>
00064 #include <tf/message_filter.h>
00065 #include <message_filters/subscriber.h>
00066 #if ROS_VERSION_MINIMUM(1,8,0) // test for Fuerte (new build system, separate stacks)
00067 #include <octomap_msgs/OctomapBinary.h>
00068 #include <octomap_msgs/GetOctomap.h>
00069 #include <octomap_msgs/BoundingBoxQuery.h>
00070 #else
00071 #include <octomap_ros/OctomapBinary.h>
00072 #include <octomap_ros/GetOctomap.h>
00073 #include <octomap_ros/ClearBBXRegion.h>
00074 #endif
00075
00076 #include <octomap_ros/conversions.h>
00077 #include <octomap/octomap.h>
00078 #include <octomap/OcTreeKey.h>
00079
00080
00081 namespace octomap_server {
00082 class OctomapServer{
00083
00084 public:
00085 typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
00086 #if ROS_VERSION_MINIMUM(1,8,0)
00087 typedef octomap_msgs::GetOctomap OctomapSrv;
00088 typedef octomap_msgs::BoundingBoxQuery BBXSrv;
00089 #else
00090 typedef octomap_ros::GetOctomap OctomapSrv;
00091 typedef octomap_ros::ClearBBXRegion BBXSrv;
00092 #endif
00093
00094 typedef octomap::OcTree OcTreeT;
00095
00096 OctomapServer(ros::NodeHandle private_nh_ = ros::NodeHandle("~"));
00097 virtual ~OctomapServer();
00098 virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res);
00099 virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res);
00100 bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp);
00101 bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
00102
00103 virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);
00104 virtual bool openFile(const std::string& filename);
00105
00106 protected:
00107 inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min){
00108 for (unsigned i=0; i<3; ++i)
00109 min[i] = std::min(in[i], min[i]);
00110 };
00111
00112 inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max){
00113 for (unsigned i=0; i<3; ++i)
00114 max[i] = std::max(in[i], max[i]);
00115 };
00116
00118 inline bool isInUpdateBBX(const octomap::OcTree::iterator& it) const{
00119
00120 unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth()));
00121 octomap::OcTreeKey key = it.getIndexKey();
00122 return (key[0]+voxelWidth >= m_updateBBXMin[0]
00123 && key[1]+voxelWidth >= m_updateBBXMin[1]
00124 && key[0] <= m_updateBBXMax[0]
00125 && key[1] <= m_updateBBXMax[1]);
00126 }
00127
00128 void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level);
00129 void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00130 void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;
00131 void publishAll(const ros::Time& rostime = ros::Time::now());
00132
00141 virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground);
00142
00144 void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const;
00145
00151 bool isSpeckleNode(const octomap::OcTreeKey& key) const;
00152
00154 virtual void handlePreNodeTraversal(const ros::Time& rostime);
00155
00157 virtual void handleNode(const OcTreeT::iterator& it) {};
00158
00160 virtual void handleNodeInBBX(const OcTreeT::iterator& it) {};
00161
00163 virtual void handleOccupiedNode(const OcTreeT::iterator& it);
00164
00166 virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it);
00167
00169 virtual void handleFreeNode(const OcTreeT::iterator& it);
00170
00172 virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it);
00173
00175 virtual void handlePostNodeTraversal(const ros::Time& rostime);
00176
00178 virtual void update2DMap(const OcTreeT::iterator& it, bool occupied);
00179
00180 inline unsigned mapIdx(int i, int j) const{
00181 return m_gridmap.info.width*j + i;
00182 }
00183
00184 inline unsigned mapIdx(const octomap::OcTreeKey& key) const{
00185 return mapIdx((key[0] - m_paddedMinKey[0])/m_multires2DScale,
00186 (key[1] - m_paddedMinKey[1])/m_multires2DScale);
00187
00188 }
00189
00196 void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const;
00197
00198 inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo){
00199 return ( oldMapInfo.height != newMapInfo.height
00200 || oldMapInfo.width !=newMapInfo.width
00201 || oldMapInfo.origin.position.x != newMapInfo.origin.position.x
00202 || oldMapInfo.origin.position.y != newMapInfo.origin.position.y);
00203 }
00204
00205 static std_msgs::ColorRGBA heightMapColor(double h);
00206 ros::NodeHandle m_nh;
00207 ros::Publisher m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub;
00208 message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub;
00209 tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub;
00210 ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService;
00211 tf::TransformListener m_tfListener;
00212 dynamic_reconfigure::Server<OctomapServerConfig> m_reconfigureServer;
00213
00214 octomap::OcTree* m_octree;
00215 octomap::KeyRay m_keyRay;
00216 octomap::OcTreeKey m_updateBBXMin;
00217 octomap::OcTreeKey m_updateBBXMax;
00218
00219 double m_maxRange;
00220 std::string m_worldFrameId;
00221 std::string m_baseFrameId;
00222 bool m_useHeightMap;
00223 std_msgs::ColorRGBA m_color;
00224 std_msgs::ColorRGBA m_colorFree;
00225 double m_colorFactor;
00226
00227 bool m_latchedTopics;
00228 bool m_publishFreeSpace;
00229
00230 double m_res;
00231 unsigned m_treeDepth;
00232 unsigned m_maxTreeDepth;
00233 double m_probHit;
00234 double m_probMiss;
00235 double m_thresMin;
00236 double m_thresMax;
00237
00238 double m_pointcloudMinZ;
00239 double m_pointcloudMaxZ;
00240 double m_occupancyMinZ;
00241 double m_occupancyMaxZ;
00242 double m_minSizeX;
00243 double m_minSizeY;
00244 bool m_filterSpeckles;
00245
00246 bool m_filterGroundPlane;
00247 double m_groundFilterDistance;
00248 double m_groundFilterAngle;
00249 double m_groundFilterPlaneDistance;
00250
00251 bool m_compressMap;
00252
00253
00254 bool m_incrementalUpdate;
00255 nav_msgs::OccupancyGrid m_gridmap;
00256 bool m_publish2DMap;
00257 bool m_mapOriginChanged;
00258 octomap::OcTreeKey m_paddedMinKey;
00259 unsigned m_multires2DScale;
00260 bool m_projectCompleteMap;
00261 };
00262 }
00263
00264 #endif