crsm_slam.h

Go to the documentation of this file.

/*
 *      This file is part of CrsmSlam.
    CrsmSlam is free software: you can redistribute it and/or modify 
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    CrsmSlam is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with CrsmSlam.  If not, see <http://www.gnu.org/licenses/>.
* 
* Author : Manos Tsardoulias, etsardou@gmail.com
* Organization : AUTH, PANDORA Robotics Team
* */

#ifndef CRSM_SLAM_HEADER
#define CRSM_SLAM_HEADER


#include <iostream>
#include <cstdlib>
#include <cmath>
#include <vector>
#include <string>

#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "ros/ros.h"

#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>

#include "crsm_laser.h"
#include "crsm_hillClimbing.h"
#include "crsm_pose.h"
#include "crsm_map.h"
#include "crsm_slamParameters.h"

namespace crsm_slam{

        class CrsmSlam{

                private:
                        ros::Subscriber clientLaserValues;                              
                        ros::Publisher _occupancyGridPublisher;                 
                        ros::Publisher _pathPublisher;                                  
                        tf::TransformBroadcaster _slamFrameBroadcaster; 
                        tf::TransformListener _listener;                                
                        
                        CrsmExpansion expansion;
                        
                        CrsmMap map;            
                        CrsmLaser laser;        
                                
                        int argc;                       
                        char **argv;            

                        unsigned int counter;   

                        float bestFitness;              
                        float meanDensity;              

                        CrsmTransformation bestTransformation;  
                        CrsmPose robotPose;                                     
                        CrsmSlamParameters slamParams;                  
                        
                        std::vector<CrsmPose> robotTrajectory;  

                        std::set<int> scanSelections;                   
                        std::set<int> bigChanges;                               
                        
                        void updateParameters(void);
                        
                        bool checkExpansion(int x,int y,bool update);
                        
                        void expandMap(void);
                        
                        ros::Timer _pathPublishingTimer;                
                        ros::Timer _robotPosePublishingTimer;   
                        ros::Timer _mapPublishingTimer;                 

                public:
                
                        ros::NodeHandle n;      
                        

                        CrsmSlam(int argc, char **argv);
                        
                        ~CrsmSlam(){}
                        
                        void updateMapProbabilities(void);

                        void calculateCriticalRays(void);

                        void findTransformation(void);
                        
                        void fixNewScans(const sensor_msgs::LaserScanConstPtr& msg);
                        
                        void startLaserSubscriber(void);
                        
                        void stopLaserSubscriber(void);

                        void startOGMPublisher(void);
                        
                        void stopOGMPublisher(void);
                        
                        void publishOGM(const ros::TimerEvent& e);
                        
                        void publishRobotPoseTf(const ros::TimerEvent& e);
                        
                        void startTrajectoryPublisher(void);
                        
                        void stopTrajectoryPublisher(void);
                        
                        void publishTrajectory(const ros::TimerEvent& e);
                        
                        CrsmMapInfo getMapInfo(void);
                        
                        char getMapProbability(int x,int y);
                        
                        CrsmPose getRobotPose(void);
                        
                        CrsmLaserInfo getLaserInfo(void);
                        
                        std::vector<CrsmPose> getTrajectory(void);
                        
                        //---------------------- Setters for slamParameters ----------------------------//
                        void setDisparity(int disparity);
                        
                        void setInitialMapSize(int size);
                        
                        void setOcgd(double ocgd);
                        
                        void setDensity(double density);
                        
                        void setObstacleDensity(double ob_density);
                        
                        void setScanSelectionMeters(double scan_selection_meters);
                        
                        void setMaxHillClimbingIterations(int iterations);
                        
                        void setDxLaserRobotCenter(double dx);
                        
                        void setOccupancyGridMapFreq(double freq);
                        
                        void setRobotPoseTfFreq(double freq);
                        
                        void setTrajectoryFreq(double freq);
                        
                        void setDesiredNumberOfPickedRays(int rays);
                        
                        void setRobotWidth(double width);
                        
                        void setRobotLength(double length);
                        
                        void setOccupancyGridPublishTopic(std::string topic);
                        
                        void setRobotTrajectoryPublishTopic(std::string topic);
                        
                        void setTrajectoryPublisherFrameId(std::string frame_id);
                        
                        void setLaserSubscriberTopic(std::string topic);
                        
                        void setWorldFrame(std::string frame);
                        
                        void setBaseFootprintFrame(std::string frame);
                        
                        void setBaseFrame(std::string frame);
                        
                        void setMapFrame(std::string frame);
                        
                        void setLaserFrame(std::string frame);
                        
                        //------------------- Getters for slamParameters ----------------------//
                        
                        int getDisparity(void);
                        
                        int getInitialMapSize(void);
                        
                        double getOcgd(void);
                        
                        double getDensity(void);
                        
                        double getObstacleDensity(void);
                        
                        double getScanSelectionMeters(void);
                        
                        int getMaxHillClimbingIterations(void);
                        
                        double getDxLaserRobotCenter(void);
                        
                        double getOccupancyGridMapFreq(void);
                        
                        double getRobotPoseTfFreq(void);
                        
                        double getTrajectoryFreq(void);
                        
                        int getDesiredNumberOfPickedRays(void);
                        
                        double getRobotWidth(void);
                        
                        double getRobotLength(void);
                        
                        std::string getOccupancyGridPublishTopic(void);
                        
                        std::string getRobotTrajectoryPublishTopic(void);
                        
                        std::string getTrajectoryPublisherFrameId(void);
                        
                        std::string getLaserSubscriberTopic(void);
                        
                        std::string getWorldFrame(void);
                        
                        std::string getBaseFootprintFrame(void);
                        
                        std::string getBaseFrame(void);
                        
                        std::string getMapFrame(void);
                        
                        std::string getLaserFrame(void);
                };

}

#endif