CGraphSlamEngine_MR.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 <ros/callback_queue.h>

#include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h"
#include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h"
#include "mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h"
#include "mrpt_graphslam_2d/CConnectionManager.h"
#include "mrpt_graphslam_2d/misc/common.h"

#include <mrpt_msgs/NodeIDWithLaserScan.h>
#include <mrpt_msgs/NodeIDWithPose_vec.h>
#include <mrpt_msgs/NetworkOfPoses.h>
#include <mrpt_msgs/GetCMGraph.h> // service
#include <mrpt_bridge/network_of_poses.h>
#include <mrpt_bridge/laser_scan.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/String.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFSOG.h>

#include <mrpt/math/utils.h>
#include <mrpt/system/os.h>
#include <mrpt/slam/CGridMapAligner.h>
#include <mrpt/graphs/TMRSlamNodeAnnotations.h>
#include <mrpt/graphslam/misc/TUncertaintyPath.h>
#include <mrpt/graphslam/misc/TNodeProps.h>

#include <mrpt/version.h>
#if MRPT_VERSION>=0x199
#include <mrpt/img/TColorManager.h>
#include <mrpt/img/TColor.h>
#include <mrpt/graphs/TNodeID.h>
#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/containers/stl_containers_utils.h>
using namespace mrpt::img;
using namespace mrpt::graphs;
using namespace mrpt::config;
using namespace mrpt::system;
using namespace mrpt::containers;
#else
#include <mrpt/utils/TColorManager.h>
using namespace mrpt::utils;
#endif


#include <set>
#include <iterator>
#include <algorithm>

namespace mrpt { namespace graphslam {

template<class GRAPH_T>
class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS<GRAPH_T>
{
public:
        typedef CGraphSlamEngine_ROS<GRAPH_T> parent_t;
        typedef CGraphSlamEngine_MR<GRAPH_T> self_t;
        typedef typename GRAPH_T::constraint_t constraint_t;
        typedef typename constraint_t::type_value pose_t;
        typedef std::pair<
                TNodeID,
                mrpt::obs::CObservation2DRangeScan::Ptr> MRPT_NodeIDWithLaserScan;
        typedef std::map<
                TNodeID,
                mrpt::obs::CObservation2DRangeScan::Ptr> nodes_to_scans2D_t;
        typedef std::vector<std::vector<TNodeID>> partitions_t;
        typedef typename mrpt::graphs::detail::THypothesis<GRAPH_T> hypot_t;
        typedef std::vector<hypot_t> hypots_t;
        typedef std::vector<hypot_t*> hypotsp_t;
        typedef typename GRAPH_T::global_pose_t global_pose_t;
        typedef typename mrpt::graphslam::detail::TNodeProps<GRAPH_T> node_props_t;
        typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> path_t;
        typedef std::vector<path_t> paths_t;
        typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR<GRAPH_T> edge_reg_mr_t;

        CGraphSlamEngine_MR(
                        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
                        );

        ~CGraphSlamEngine_MR();

        bool _execGraphSlamStep(
                        mrpt::obs::CActionCollection::Ptr& action,
                        mrpt::obs::CSensoryFrame::Ptr& observations,
                        mrpt::obs::CObservation::Ptr& observation,
                        size_t& rawlog_entry);

        void initClass();

        struct TNeighborAgentProps {
                TNeighborAgentProps(
                                CGraphSlamEngine_MR<GRAPH_T>& engine_in,
                                const mrpt_msgs::GraphSlamAgent& agent_in);
                ~TNeighborAgentProps();

                void setupComm();
                void setupSubs();
                void setupSrvs();
                void fetchUpdatedNodesList(
                                const mrpt_msgs::NodeIDWithPose_vec::ConstPtr& nodes);
                void fetchLastRegdIDScan(
                                const mrpt_msgs::NodeIDWithLaserScan::ConstPtr& last_regd_id_scan);
                void getCachedNodes(
                                std::vector<TNodeID>* nodeIDs=NULL,
                                std::map<
                                        TNodeID,
                                        node_props_t>* nodes_params=NULL,
                                bool only_unused=true) const;
                void fillOptPaths(
                                const std::set<TNodeID>& nodeIDs,
                                paths_t* opt_paths) const;
                void computeGridMap() const;
                const mrpt::maps::COccupancyGridMap2D::Ptr& getGridMap() const;
                void getGridMap(mrpt::maps::COccupancyGridMap2D::Ptr& map) const;
                bool hasNewData() const;
                std::string getAgentNs() const { return this->agent.topic_namespace.data; }
                void resetFlags() const;
                bool operator==(
                                const TNeighborAgentProps& other) const {
                        return (this->agent == other.agent);
                }
                bool operator<(
                                const TNeighborAgentProps& other) const {
                        return (this->agent < other.agent);
                }
                const sensor_msgs::LaserScan* getLaserScanByNodeID(
                                const TNodeID nodeID) const;

                CGraphSlamEngine_MR<GRAPH_T>& engine;

                const mrpt_msgs::GraphSlamAgent agent;
                bool hasNewNodesBatch(int new_batch_size);

                void setTColor(const TColor& color_in) { color = color_in; }
                TColor color;

                std::set<TNodeID> nodeIDs_set;
                typename GRAPH_T::global_poses_t poses;
                std::vector<mrpt_msgs::NodeIDWithLaserScan> ros_scans;
                std::map<TNodeID, bool> nodeID_to_is_integrated;
                ros::Subscriber last_regd_nodes_sub;
                ros::Subscriber last_regd_id_scan_sub;

                ros::ServiceClient cm_graph_srvclient;
                std::string last_regd_nodes_topic;
                std::string last_regd_id_scan_topic;

                std::string cm_graph_service;
                mutable mrpt::maps::COccupancyGridMap2D::Ptr gridmap_cached;

                mutable bool has_new_nodes;
                mutable bool has_new_scans;

                int m_queue_size;
                ros::NodeHandle* nh;
                bool has_setup_comm;

                mrpt::poses::CPose2D tf_self_to_neighbor_first_integrated_pose;
                std::pair<
                        TNodeID,
                        mrpt::poses::CPose2D> last_integrated_pair_neighbor_frame;

        };
        typedef std::vector<TNeighborAgentProps*> neighbors_t;

        const neighbors_t& getVecOfNeighborAgentProps() const {
                return m_neighbors;
        }
        bool isOwnNodeID(
                        const TNodeID nodeID,
                        const global_pose_t* pose_out=NULL) const;


private:
        bool addNodeBatchesFromAllNeighbors();
        bool addNodeBatchFromNeighbor(TNeighborAgentProps* neighbor);
        bool findTFsWithAllNeighbors();
        bool findTFWithNeighbor(TNeighborAgentProps* neighbor);
        bool getNeighborByAgentID(
                        const std::string& agent_ID_str,
                        TNeighborAgentProps*& neighbor) const;
        bool pubUpdatedNodesList();
        bool pubLastRegdIDScan();

        void usePublishersBroadcasters();

        void setupSubs();
        void setupPubs();
        void setupSrvs();

        bool getCMGraph(
                        mrpt_msgs::GetCMGraph::Request& req,
                        mrpt_msgs::GetCMGraph::Response& res);

        void readParams();
        void readROSParameters();
        void printParams() const;
        mrpt::poses::CPose3D getLSPoseForGridMapVisualization(
                        const TNodeID nodeID) const;
        void setObjectPropsFromNodeID(
                        const TNodeID nodeID,
                        mrpt::opengl::CSetOfObjects::Ptr& viz_object);
        void monitorNodeRegistration(
                        bool registered=false,
                        std::string class_name="Class");
        void getAllOwnNodes(
                std::set<TNodeID>* nodes_set) const;
        void getNodeIDsOfEstimatedTrajectory(
                std::set<TNodeID>* nodes_set) const;
        void getRobotEstimatedTrajectory(
                        typename GRAPH_T::global_poses_t* graph_poses) const;


        neighbors_t m_neighbors;
        std::map<TNeighborAgentProps*, bool> m_neighbor_to_found_initial_tf;

        ros::Publisher m_list_neighbors_pub;
        ros::Publisher m_last_regd_id_scan_pub;
        ros::Publisher m_last_regd_nodes_pub;

        ros::ServiceServer m_cm_graph_srvserver;
        std::string m_mr_ns;
        std::string m_list_neighbors_topic;
        std::string m_last_regd_id_scan_topic;
        std::string m_last_regd_nodes_topic;

        std::string m_cm_graph_service;

        size_t m_nodes_to_laser_scans2D_last_size;

        mrpt::graphslam::detail::CConnectionManager m_conn_manager;


        ros::NodeHandle* m_nh;
        size_t m_graph_nodes_last_size;

        double m_offset_y_nrd;
        double m_offset_y_erd;
        double m_offset_y_gso;
        double m_offset_y_namespace;

        int m_text_index_nrd;
        int m_text_index_erd;
        int m_text_index_gso;
        int m_text_index_namespace;
        bool m_registered_multiple_nodes;

        ros::AsyncSpinner cm_graph_async_spinner;

        mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options;

        TColorManager m_neighbor_colors_manager;

        std::string m_sec_alignment_params;
        std::string m_sec_mr_slam_params;

        struct TOptions : CLoadableOptions {

                typedef self_t engine_mr_t;

                TOptions(const engine_mr_t& engine_in);
                ~TOptions();
                void loadFromConfigFile(
                                const CConfigFileBase& source,
                                const std::string& section);
#if MRPT_VERSION>=0x199
                void dumpToTextStream(std::ostream& out) const;
#else
                void dumpToTextStream(mrpt::utils::CStream& out) const;
#endif


                bool conservative_find_initial_tfs_to_neighbors;
                int nodes_integration_batch_size;
                int num_last_regd_nodes;
                size_t inter_group_node_count_thresh;
                size_t inter_group_node_count_thresh_minadv;

                const engine_mr_t& engine;

        } m_opts;

};


} } // end of namespaces

// pseudo-split decleration from implementation
#include "mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h"