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/NodeIDWithPoseVec.h>
#include <mrpt_msgs/NodeIDWithLaserScan.h>
#include <mrpt_msgs/NetworkOfPoses.h>
#include <mrpt_msgs/GetCMGraph.h>  // service
#include <mrpt_msgs_bridge/network_of_poses.h>
#include <mrpt/ros1bridge/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/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>

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

using namespace mrpt::img;
using namespace mrpt::graphs;
using namespace mrpt::config;
using namespace mrpt::system;
using namespace mrpt::containers;

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::NodeIDWithPoseVec::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);
                void dumpToTextStream(std::ostream& out) const;

                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;
};

}  // namespace graphslam
}  // namespace mrpt

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