sensor_map.hpp

Go to the documentation of this file.

#ifndef mitre_fast_layered_map_sensor_map
#define mitre_fast_layered_map_sensor_map
#pragma once

// STD
#include <string>

// ROS
#include <filters/filter_chain.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf2_ros/buffer.h> // Transform point clouds
#include <tf2_ros/transform_listener.h>

// Grid map
#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_msgs/GridMap.h>

// Pcl
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl-1.7/pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

namespace mitre_fast_layered_map
{

    struct MapConfiguration
    {
        // SUBSCRIBED TOPICS
        std::string nonGroundPointSubTopic; 
        std::string groundPointSubTopic;    
        std::string odomSubTopic;           
        std::string markerSubTopic;         
        std::string staticMapSubTopic;      
        std::string occupancyOutputTopic;   
        std::string gridmapOutputTopic;     

        // MAP CHARACTERISTICS
        std::string mapName;    
        std::string mapFrameId; 
        grid_map::Length len;   
        double resolution;      
        std::string historyLayerPrefix; 
        int numHistoryLayers;   
        

        // VEHICLE CHARACTERISTICS
        // @TODO: Could this be pulled from the robot model automatically?
        // Used to remove lidar points that may be hitting the vehicle
        std::string vehicleFrameId; 
        double footPrintLen;        
        double footPrintWidth;      

        // FILTERS
        std::string obstacleFilterNs; 
        std::string mapOperationsFilterNs; 
        bool enablePointHeightFilter; 
        bool enableBoundingBoxFilter; 
        double maxPointHeight;        

        // SAVING OBSTACLES
        bool enablePermanentObstacles; 
        double permanentFilterProb;   
    };

    inline bool operator==(const MapConfiguration &conf1, const MapConfiguration &conf2)
    {
        return (conf1.nonGroundPointSubTopic == conf2.nonGroundPointSubTopic &&
                conf1.groundPointSubTopic == conf2.groundPointSubTopic &&
                conf1.odomSubTopic == conf2.odomSubTopic &&
                conf1.markerSubTopic == conf2.markerSubTopic &&
                conf1.staticMapSubTopic == conf2.staticMapSubTopic &&
                conf1.occupancyOutputTopic == conf2.occupancyOutputTopic &&
                conf1.mapName == conf2.mapName &&
                conf1.mapFrameId == conf2.mapFrameId &&
                conf1.len(0) == conf2.len(0) &&
                conf1.resolution == conf2.resolution &&
                conf1.historyLayerPrefix == conf2.historyLayerPrefix &&
                conf1.numHistoryLayers == conf2.numHistoryLayers &&
                conf1.vehicleFrameId == conf2.vehicleFrameId &&
                conf1.enableBoundingBoxFilter == conf2.enableBoundingBoxFilter &&
                conf1.footPrintLen == conf2.footPrintLen &&
                conf1.footPrintWidth == conf2.footPrintWidth &&
                conf1.obstacleFilterNs == conf2.obstacleFilterNs &&
                conf1.mapOperationsFilterNs == conf2.mapOperationsFilterNs &&
                conf1.enablePointHeightFilter == conf2.enablePointHeightFilter &&
                conf1.maxPointHeight == conf2.maxPointHeight &&
                conf1.enablePermanentObstacles == conf2.enablePermanentObstacles &&
                conf1.permanentFilterProb == conf2.permanentFilterProb);
    }

    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
    class SensorMap
    {
    public:
        SensorMap();

        SensorMap(ros::NodeHandle *, MapConfiguration);

        ~SensorMap();

        int init();
        int once();

        void odomCb(const nav_msgs::Odometry::ConstPtr);
        int moveMap(double, double);

        void groundPointCb(const sensor_msgs::PointCloud2 &);
        int updateGroundPts(const PointCloud &);

        void nonGroundPointCb(const sensor_msgs::PointCloud2 &);
        int updateNongroundPts(const PointCloud &);
        bool pointHeightFilter(const pcl::PointXYZ, double _vehicleHeight = 0);
        bool pointBoundingBoxFilter(const pcl::PointXYZ);

        int integrateStaticMap();
        void markerCb(const visualization_msgs::Marker &);

        void staticMapCb(const nav_msgs::OccupancyGrid &);

        int getOccupancyGrid(nav_msgs::OccupancyGrid &);
        int publishMap();

        int tfTransformCloud(const sensor_msgs::PointCloud2 &, sensor_msgs::PointCloud2 &, std::string);

        friend class TestMap;

    private:
        int runFilter();

        tf2_ros::Buffer tfBuffer_;              
        tf2_ros::TransformListener tfListener_; 
        bool initialized_{false};              // Stateful variable for determining if map has been initialized

        MapConfiguration config_;

        ros::NodeHandle nh_;                 
        ros::Subscriber markerSub_;          
        ros::Subscriber odomSub_;            
        ros::Subscriber nonGroundPointsSub_; 
        ros::Subscriber groundPointsSub_;    
        ros::Subscriber staticMapSub_;       
        ros::Publisher occPub_;              
        ros::Publisher gridMapPub_;          

        grid_map::GridMap gridMap_;          
        grid_map::GridMap staticMap_;        
        filters::FilterChain<grid_map::GridMap> obstacleFilterChain_; 
        filters::FilterChain<grid_map::GridMap> mapOperationsFilterChain_; 
        uint64_t readingsReceived_{0}; 
        bool cornersCalculated_{false}; 
        std::vector<geometry_msgs::Pose> corners_; 
        bool recStaticMap_{false}; 
        ros::Time odomLastUpdate_;     
        ros::Time groundLastUpdate_; 
        ros::Time nongroundLastUpdate_; 
    };

    // Class with helper functions for testing that the map above works as expected
    class TestMap
    {

    public:
        TestMap();
        ~TestMap();

        bool CheckConfigEqual(SensorMap &, MapConfiguration &);
        bool CheckMapsEqual(SensorMap &, grid_map::GridMap &);
        bool CheckGeometry(SensorMap &, grid_map::Length, double resolution);
        bool CheckFrame(SensorMap &, std::string);
        bool CheckPosition(SensorMap &, double x, double y);
        bool CheckNans(SensorMap &);
        bool TestMapCells(SensorMap &, std::string, Eigen::MatrixXi);
        bool TestMapCells(SensorMap& , std::string, Eigen::MatrixXf);
    };
} // namespace mitre_fast_layered_map

#endif