CGraphSlamHandler_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

// ROS
#include <ros/ros.h>
#include <mrpt_bridge/mrpt_bridge.h>
#include <mrpt_msgs/GraphSlamStats.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/Header.h>

#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>

// MRPT
#include <mrpt/graphs/CNetworkOfPoses.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CObservation2DRangeScan.h>

#include <mrpt/system/string_utils.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/utils/COutputLogger.h>
#include <mrpt/graphslam/apps_related/CGraphSlamHandler.h>

#include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h"
#include "mrpt_graphslam_2d/CGraphSlamEngine_MR.h"
#include "mrpt_graphslam_2d/TUserOptionsChecker_ROS.h"


// cpp headers
#include <string>
#include <sstream>
#include <vector>

namespace mrpt { namespace graphslam { namespace apps {

template<class GRAPH_T=mrpt::graphs::CNetworkOfPoses2DInf>
class CGraphSlamHandler_ROS :
        public CGraphSlamHandler<GRAPH_T>
{
        public:
                typedef typename GRAPH_T::constraint_t constraint_t;
                typedef typename GRAPH_T::constraint_t::type_value pose_t;

                typedef CGraphSlamHandler_ROS<GRAPH_T> self_t;
                typedef CGraphSlamHandler<GRAPH_T> parent_t;

                CGraphSlamHandler_ROS(
                                mrpt::utils::COutputLogger* logger,
                                TUserOptionsChecker<GRAPH_T>* options_checker,
                                ros::NodeHandle* nh_in);
                ~CGraphSlamHandler_ROS();


                void getParamsAsString(std::string* str_out);
                std::string getParamsAsString();

                void readParams();
                void printParams();

                void sniffOdom(const nav_msgs::Odometry::ConstPtr& ros_odom);
                void sniffLaserScan(const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan);
                void sniffCameraImage();
                void sniff3DPointCloud();
                bool continueExec();
                void generateReport();
                bool usePublishersBroadcasters();
                void setupComm();

                void initEngine_ROS();
                void initEngine_MR();
                static const std::string sep_header;
                static const std::string sep_subheader;

        private:
                void processObservation(mrpt::obs::CObservation::Ptr& observ);
                void _process(mrpt::obs::CObservation::Ptr& observ);
                void readROSParameters();
                void readStaticTFs();
                void getROSParameters(std::string* str_out);
                void verifyUserInput();

                void resetReceivedFlags();
                void setupSubs();
                void setupPubs();
                void setupSrvs();
                ros::NodeHandle* m_nh;

                // ROS server parameters
                std::string m_node_reg;
                std::string m_edge_reg;
                std::string m_optimizer;
                VerbosityLevel m_min_logging_level;

                bool m_received_odom;
                bool m_received_laser_scan;
                bool m_received_camera;
                bool m_received_point_cloud;
                mrpt::obs::CObservationOdometry::Ptr m_mrpt_odom;
                mrpt::obs::CObservation2DRangeScan::Ptr m_mrpt_laser_scan;
                ros::Subscriber m_odom_sub;
                ros::Subscriber m_laser_scan_sub;
                ros::Subscriber m_camera_scan_sub;
                ros::Subscriber m_point_cloud_scan_sub;

                ros::Publisher m_curr_robot_pos_pub;
                ros::Publisher m_robot_trajectory_pub;
                ros::Publisher m_robot_tr_poses_pub;
                ros::Publisher m_gt_trajectory_pub; // TODO
                ros::Publisher m_SLAM_eval_metric_pub; // TODO
                ros::Publisher m_odom_trajectory_pub;
                ros::Publisher m_gridmap_pub;
                ros::Publisher m_stats_pub;
                std::string m_odom_topic;
                std::string m_laser_scan_topic;
                std::string m_camera_topic;
                std::string m_point_cloud_topic;

                std::string m_curr_robot_pos_topic;
                std::string m_robot_trajectory_topic;
                std::string m_robot_tr_poses_topic;
                std::string m_odom_trajectory_topic;
                std::string m_gridmap_topic;
                std::string m_stats_topic;
                tf2_ros::Buffer m_buffer;
                tf2_ros::TransformBroadcaster m_broadcaster;
                std::string m_anchor_frame_id;
                std::string m_base_link_frame_id;
                std::string m_odom_frame_id;
                geometry_msgs::TransformStamped m_anchor_odom_transform;
                nav_msgs::Path m_odom_path;

                int m_pub_seq;
                int m_stats_pub_seq;

                size_t m_measurement_cnt;

                int m_queue_size;

                size_t m_graph_nodes_last_size;

                bool m_first_time_in_sniff_odom;
                pose_t m_input_odometry_offset;
};

} } } // end of namespaces

#include "mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h"