MapsManager.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #ifndef MAPSMANAGER_H_
00029 #define MAPSMANAGER_H_
00030 
00031 #include <rtabmap/core/Signature.h>
00032 #include <rtabmap/core/Parameters.h>
00033 #include <rtabmap/core/FlannIndex.h>
00034 #include <pcl/point_cloud.h>
00035 #include <pcl/point_types.h>
00036 #include <ros/time.h>
00037 #include <ros/publisher.h>
00038 
00039 namespace rtabmap {
00040 class OctoMap;
00041 class Memory;
00042 class OccupancyGrid;
00043 
00044 }  // namespace rtabmap
00045 
00046 class MapsManager {
00047 public:
00048         MapsManager();
00049         virtual ~MapsManager();
00050         void init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace);
00051         void clear();
00052         bool hasSubscribers() const;
00053         void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const;
00054         void setParameters(const rtabmap::ParametersMap & parameters);
00055         void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses, const rtabmap::Memory * memory = 0);
00056 
00057         std::map<int, rtabmap::Transform> getFilteredPoses(
00058                         const std::map<int, rtabmap::Transform> & poses);
00059 
00060         std::map<int, rtabmap::Transform> updateMapCaches(
00061                         const std::map<int, rtabmap::Transform> & poses,
00062                         const rtabmap::Memory * memory,
00063                         bool updateGrid,
00064                         bool updateOctomap,
00065                         const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
00066 
00067         void publishMaps(
00068                         const std::map<int, rtabmap::Transform> & poses,
00069                         const ros::Time & stamp,
00070                         const std::string & mapFrameId);
00071 
00072         cv::Mat getGridMap(
00073                         float & xMin,
00074                         float & yMin,
00075                         float & gridCellSize);
00076 
00077         cv::Mat getGridProbMap(
00078                         float & xMin,
00079                         float & yMin,
00080                         float & gridCellSize);
00081 
00082         const rtabmap::OctoMap * getOctomap() const {return octomap_;}
00083         const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
00084 
00085 private:
00086         // mapping stuff
00087         bool cloudOutputVoxelized_;
00088         bool cloudSubtractFiltering_;
00089         int cloudSubtractFilteringMinNeighbors_;
00090         double mapFilterRadius_;
00091         double mapFilterAngle_;
00092         bool mapCacheCleanup_;
00093         bool negativePosesIgnored_;
00094         bool negativeScanEmptyRayTracing_;
00095 
00096         ros::Publisher cloudMapPub_;
00097         ros::Publisher cloudGroundPub_;
00098         ros::Publisher cloudObstaclesPub_;
00099         ros::Publisher projMapPub_;
00100         ros::Publisher gridMapPub_;
00101         ros::Publisher gridProbMapPub_;
00102         ros::Publisher scanMapPub_;
00103         ros::Publisher octoMapPubBin_;
00104         ros::Publisher octoMapPubFull_;
00105         ros::Publisher octoMapCloud_;
00106         ros::Publisher octoMapGroundCloud_;
00107         ros::Publisher octoMapObstacleCloud_;
00108         ros::Publisher octoMapEmptySpace_;
00109         ros::Publisher octoMapProj_;
00110 
00111         std::map<int, rtabmap::Transform> assembledGroundPoses_;
00112         std::map<int, rtabmap::Transform> assembledObstaclePoses_;
00113         pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
00114         pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
00115         rtabmap::FlannIndex assembledGroundIndex_;
00116         rtabmap::FlannIndex assembledObstacleIndex_;
00117         std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > groundClouds_;
00118         std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > obstacleClouds_;
00119 
00120         std::map<int, rtabmap::Transform> gridPoses_;
00121         cv::Mat gridMap_;
00122         std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> > gridMaps_; // < <ground, obstacles>, empty cells >
00123         std::map<int, cv::Point3f> gridMapsViewpoints_;
00124 
00125         rtabmap::OccupancyGrid * occupancyGrid_;
00126 
00127         rtabmap::OctoMap * octomap_;
00128         int octomapTreeDepth_;
00129 
00130         rtabmap::ParametersMap parameters_;
00131 };
00132 
00133 #endif /* MAPSMANAGER_H_ */


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:30:49