CGraphSlamEngine_ROS.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
         |                     Mobile Robot Programming Toolkit (MRPT)               |
         |                          http://www.mrpt.org/                             |
         |                                                                           |
         | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
         | See: http://www.mrpt.org/Authors - All rights reserved.                   |
         | Released under BSD License. See details in http://www.mrpt.org/License    |
         +---------------------------------------------------------------------------+ */

#pragma once

#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_ROS.h"

#include <mrpt/graphslam/CGraphSlamEngine.h>
#include <ros/ros.h>

namespace mrpt { namespace graphslam {

template<class GRAPH_t=typename mrpt::graphs::CNetworkOfPoses2DInf>
class CGraphSlamEngine_ROS : public CGraphSlamEngine<GRAPH_t>
{
public:
        typedef CGraphSlamEngine<GRAPH_t> parent;

        CGraphSlamEngine_ROS(
                        ros::NodeHandle* nh,
                        const std::string& config_file,
                        const std::string& rawlog_fname="",
                        const std::string& fname_GT="",
                        mrpt::graphslam::CWindowManager* win_manager=NULL,
                        mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_t>* node_reg=NULL,
                        mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_t>* edge_reg=NULL,
                        mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_t>* optimizer=NULL
                        );
        virtual ~CGraphSlamEngine_ROS();
        void setupComm();

        void initClass();
        ros::NodeHandle* m_nh;
protected:
        void readParams();
        virtual void usePublishersBroadcasters();
        void readROSParameters();
        virtual bool _execGraphSlamStep(
                        mrpt::obs::CActionCollection::Ptr& action,
                        mrpt::obs::CSensoryFrame::Ptr& observations,
                        mrpt::obs::CObservation::Ptr& observation,
                        size_t& rawlog_entry);

        virtual void setupSubs();
        virtual void setupPubs();
        virtual void setupSrvs();
        ros::CallbackQueue custom_service_queue;


        int m_queue_size;
};

} } // end of namespaces

#include "mrpt_graphslam_2d/CGraphSlamEngine_ROS_impl.h"