00001 /* +---------------------------------------------------------------------------+ 00002 | Mobile Robot Programming Toolkit (MRPT) | 00003 | http://www.mrpt.org/ | 00004 | | 00005 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 00006 | See: http://www.mrpt.org/Authors - All rights reserved. | 00007 | Released under BSD License. See details in http://www.mrpt.org/License | 00008 +---------------------------------------------------------------------------+ */ 00009 00010 #ifndef CGRAPHSLAMHANDLER_ROS_H 00011 #define CGRAPHSLAMHANDLER_ROS_H 00012 00013 // ROS 00014 #include <ros/ros.h> 00015 #include <mrpt_bridge/mrpt_bridge.h> 00016 #include <mrpt_msgs/GraphSlamStats.h> 00017 #include <geometry_msgs/PoseStamped.h> 00018 #include <geometry_msgs/PoseArray.h> 00019 #include <geometry_msgs/TransformStamped.h> 00020 #include <sensor_msgs/LaserScan.h> 00021 #include <nav_msgs/Path.h> 00022 #include <nav_msgs/Odometry.h> 00023 #include <nav_msgs/OccupancyGrid.h> 00024 #include <std_msgs/Header.h> 00025 00026 #include <tf/transform_datatypes.h> 00027 #include <tf2_ros/buffer.h> 00028 #include <tf2_ros/transform_broadcaster.h> 00029 #include <tf2_ros/transform_listener.h> 00030 #include <tf2/LinearMath/Quaternion.h> 00031 #include <tf2/LinearMath/Vector3.h> 00032 00033 // MRPT 00034 #include <mrpt/graphs/CNetworkOfPoses.h> 00035 #include <mrpt/maps/COccupancyGridMap2D.h> 00036 #include <mrpt/obs/CObservationOdometry.h> 00037 #include <mrpt/obs/CObservation2DRangeScan.h> 00038 #include <mrpt/system/threads.h> 00039 #include <mrpt/system/string_utils.h> 00040 #include <mrpt/system/filesystem.h> 00041 #include <mrpt/system/os.h> 00042 #include <mrpt/utils/mrpt_macros.h> 00043 #include <mrpt/utils/COutputLogger.h> 00044 #include <mrpt/graphslam/apps_related/CGraphSlamHandler.h> 00045 00046 #include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h" 00047 #include "mrpt_graphslam_2d/CGraphSlamEngine_MR.h" 00048 #include "mrpt_graphslam_2d/TUserOptionsChecker_ROS.h" 00049 00050 00051 // cpp headers 00052 #include <string> 00053 #include <sstream> 00054 #include <vector> 00055 00056 namespace mrpt { namespace graphslam { namespace apps { 00057 00061 template<class GRAPH_T=mrpt::graphs::CNetworkOfPoses2DInf> 00062 class CGraphSlamHandler_ROS : 00063 public CGraphSlamHandler<GRAPH_T> 00064 { 00065 public: 00067 typedef typename GRAPH_T::constraint_t constraint_t; 00069 typedef typename GRAPH_T::constraint_t::type_value pose_t; 00070 00072 typedef CGraphSlamHandler_ROS<GRAPH_T> self_t; 00074 typedef CGraphSlamHandler<GRAPH_T> parent_t; 00075 00076 CGraphSlamHandler_ROS( 00077 mrpt::utils::COutputLogger* logger, 00078 TUserOptionsChecker<GRAPH_T>* options_checker, 00079 ros::NodeHandle* nh_in); 00080 ~CGraphSlamHandler_ROS(); 00081 00082 00083 void getParamsAsString(std::string* str_out); 00084 std::string getParamsAsString(); 00085 00090 void readParams(); 00094 void printParams(); 00095 00104 void sniffOdom(const nav_msgs::Odometry::ConstPtr& ros_odom); 00108 void sniffLaserScan(const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan); 00109 void sniffCameraImage(); 00111 void sniff3DPointCloud(); 00116 bool continueExec(); 00120 void generateReport(); 00131 bool usePublishersBroadcasters(); 00140 void setupComm(); 00141 00149 void initEngine_ROS(); 00150 void initEngine_MR(); 00153 static const std::string sep_header; 00154 static const std::string sep_subheader; 00155 00156 private: 00166 void processObservation(mrpt::obs::CObservationPtr& observ); 00172 void _process(mrpt::obs::CObservationPtr& observ); 00177 void readROSParameters(); 00178 void readStaticTFs(); 00184 void getROSParameters(std::string* str_out); 00188 void verifyUserInput(); 00189 00193 void resetReceivedFlags(); 00201 void setupSubs(); 00202 void setupPubs(); 00203 void setupSrvs(); 00206 ros::NodeHandle* m_nh; 00207 00208 // ROS server parameters 00211 std::string m_node_reg; 00212 std::string m_edge_reg; 00213 std::string m_optimizer; 00223 mrpt::utils::VerbosityLevel m_min_logging_level; 00224 00231 bool m_received_odom; 00232 bool m_received_laser_scan; 00233 bool m_received_camera; 00234 bool m_received_point_cloud; 00244 mrpt::obs::CObservationOdometryPtr m_mrpt_odom; 00245 mrpt::obs::CObservation2DRangeScanPtr m_mrpt_laser_scan; 00253 ros::Subscriber m_odom_sub; 00254 ros::Subscriber m_laser_scan_sub; 00255 ros::Subscriber m_camera_scan_sub; 00256 ros::Subscriber m_point_cloud_scan_sub; 00257 00258 ros::Publisher m_curr_robot_pos_pub; 00259 ros::Publisher m_robot_trajectory_pub; 00260 ros::Publisher m_robot_tr_poses_pub; 00261 ros::Publisher m_gt_trajectory_pub; // TODO 00262 ros::Publisher m_SLAM_eval_metric_pub; // TODO 00263 ros::Publisher m_odom_trajectory_pub; 00264 ros::Publisher m_gridmap_pub; 00265 ros::Publisher m_stats_pub; 00273 std::string m_odom_topic; 00274 std::string m_laser_scan_topic; 00275 std::string m_camera_topic; 00276 std::string m_point_cloud_topic; 00277 00278 std::string m_curr_robot_pos_topic; 00279 std::string m_robot_trajectory_topic; 00280 std::string m_robot_tr_poses_topic; 00281 std::string m_odom_trajectory_topic; 00282 std::string m_gridmap_topic; 00283 std::string m_stats_topic; 00289 tf2_ros::Buffer m_buffer; 00290 tf2_ros::TransformBroadcaster m_broadcaster; 00300 std::string m_anchor_frame_id; 00301 std::string m_base_link_frame_id; 00302 std::string m_odom_frame_id; 00310 geometry_msgs::TransformStamped m_anchor_odom_transform; 00316 nav_msgs::Path m_odom_path; 00317 00320 int m_pub_seq; 00321 int m_stats_pub_seq; 00322 00325 size_t m_measurement_cnt; 00326 00328 int m_queue_size; 00329 00330 size_t m_graph_nodes_last_size; 00331 00336 bool m_first_time_in_sniff_odom; 00337 pose_t m_input_odometry_offset; 00338 }; 00339 00340 } } } // end of namespaces 00341 00342 #include "mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h" 00343 00344 #endif /* end of include guard: CGRAPHSLAMHANDLER_ROS_H */