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_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 <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.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/system/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::system::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;
};

}  // namespace apps
}  // namespace graphslam
}  // namespace mrpt

#include "mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h"