00001 /* 00002 * File: mrpt_icp_slam_2d_wrapper.h 00003 * Author: Vladislav Tananaev 00004 * 00005 */ 00006 00007 #pragma once 00008 00009 // MRPT libraries 00010 #include <mrpt/slam/CMetricMapBuilderICP.h> 00011 #include <mrpt/opengl/COpenGLScene.h> 00012 #include <mrpt/opengl/CGridPlaneXY.h> 00013 #include <mrpt/opengl/stock_objects.h> 00014 #include <mrpt/utils/CConfigFile.h> 00015 #include <mrpt/utils/CFileGZInputStream.h> 00016 #include <mrpt/utils/CFileGZOutputStream.h> 00017 #include <mrpt/system/os.h> 00018 #include <mrpt/system/filesystem.h> 00019 #include <mrpt/opengl/CPlanarLaserScan.h> // This class lives in the lib [mrpt-maps] and must be included by hand 00020 #include <mrpt/poses/CPosePDFGaussian.h> 00021 #include <mrpt/poses/CPose3DPDF.h> 00022 00023 #include <mrpt/gui/CDisplayWindow3D.h> 00024 00025 #include <stdint.h> 00026 #include <iostream> // std::cout 00027 #include <fstream> // std::ifstream 00028 #include <string> 00029 00030 // add ros libraries 00031 #include <ros/ros.h> 00032 #include <ros/package.h> 00033 #include <tf/transform_listener.h> 00034 #include <tf/transform_broadcaster.h> 00035 // add ros msgs 00036 #include <nav_msgs/OccupancyGrid.h> 00037 #include "nav_msgs/MapMetaData.h" 00038 #include <nav_msgs/Path.h> 00039 #include <std_msgs/String.h> 00040 #include <std_msgs/Header.h> 00041 #include <std_msgs/Int32.h> 00042 #include <nav_msgs/GetMap.h> 00043 #include <geometry_msgs/PoseArray.h> 00044 #include <geometry_msgs/PoseStamped.h> 00045 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00046 #include <sensor_msgs/LaserScan.h> 00047 #include <sensor_msgs/PointCloud.h> 00048 #include <visualization_msgs/MarkerArray.h> 00049 #include <visualization_msgs/Marker.h> 00050 00051 // mrpt bridge libs 00052 #include <mrpt_bridge/pose.h> 00053 #include <mrpt_bridge/map.h> 00054 #include <mrpt_bridge/mrpt_log_macros.h> 00055 #include <mrpt_bridge/laser_scan.h> 00056 #include <mrpt_bridge/time.h> 00057 #include <mrpt_bridge/point_cloud.h> 00058 #include <mrpt/version.h> 00059 #if MRPT_VERSION >= 0x130 00060 #include <mrpt/obs/CActionRobotMovement2D.h> 00061 #include <mrpt/obs/CActionRobotMovement3D.h> 00062 #include <mrpt/obs/CActionCollection.h> 00063 #include <mrpt/obs/CObservationOdometry.h> 00064 #include <mrpt/obs/CSensoryFrame.h> 00065 #include <mrpt/maps/CMultiMetricMap.h> 00066 #include <mrpt/obs/CRawlog.h> 00067 using namespace mrpt::maps; 00068 using namespace mrpt::obs; 00069 #else 00070 #include <mrpt/slam/CActionRobotMovement2D.h> 00071 #include <mrpt/slam/CActionRobotMovement3D.h> 00072 #include <mrpt/slam/CActionCollection.h> 00073 #include <mrpt/slam/CObservationOdometry.h> 00074 #include <mrpt/slam/CSensoryFrame.h> 00075 #include <mrpt/slam/CMultiMetricMap.h> 00076 #include <mrpt/slam/CRawlog.h> 00077 #endif 00078 using namespace mrpt; 00079 using namespace mrpt::slam; 00080 using namespace mrpt::opengl; 00081 using namespace mrpt::gui; 00082 using namespace mrpt::system; 00083 using namespace mrpt::math; 00084 using namespace mrpt::utils; 00085 using namespace mrpt::poses; 00086 using namespace std; 00087 00092 class ICPslamWrapper 00093 { 00094 public: 00098 ICPslamWrapper(); 00099 00103 ~ICPslamWrapper(); 00104 00110 void read_iniFile(std::string ini_filename); 00114 void init3Dwindow(); 00118 void run3Dwindow(); 00119 00123 void get_param(); 00127 void init(); 00133 bool rawlogPlay(); 00139 bool is_file_exists(const std::string &name); 00149 void laserCallback(const sensor_msgs::LaserScan &_msg); 00154 void publishTF(); 00159 void publishMapPose(); 00165 void updateSensorPose(std::string _frame_id); 00171 void updateTrajectoryTimerCallback(const ros::TimerEvent& event); 00177 void publishTrajectoryTimerCallback(const ros::TimerEvent& event); 00178 00179 protected: 00180 CMetricMapBuilderICP mapBuilder; 00181 ros::NodeHandle n_; 00182 double rawlog_play_delay; 00183 bool rawlog_play_; 00184 00185 std::string rawlog_filename; 00186 std::string ini_filename; 00187 std::string global_frame_id; 00188 std::string odom_frame_id; 00189 std::string base_frame_id; 00190 geometry_msgs::PoseStamped pose; 00191 00192 ros::Publisher trajectory_pub_; 00193 nav_msgs::Path path; 00194 00195 ros::Timer update_trajector_timer; 00196 ros::Timer publish_trajectory_timer; 00197 00198 double trajectory_update_rate; 00199 double trajectory_publish_rate; 00200 00201 // Sensor source 00202 std::string sensor_source; 00203 std::map<std::string, mrpt::poses::CPose3D> laser_poses_; 00204 00205 // Subscribers 00206 std::vector<ros::Subscriber> sensorSub_; 00207 00208 const CMultiMetricMap *metric_map_; 00209 // CPose3DPDF::Ptr curPDF; ///<current robot pose 00210 ros::Publisher pub_map_, pub_metadata_, pub_pose_, pub_point_cloud_; 00211 00212 tf::TransformListener listenerTF_; 00213 tf::TransformBroadcaster tf_broadcaster_; 00214 00215 CTicTac tictac; 00216 float t_exec; 00217 CSensoryFrame::Ptr observations; 00218 CObservation::Ptr observation; 00219 mrpt::system::TTimeStamp timeLastUpdate_; 00220 00221 ros::Time stamp; 00222 00223 mrpt::gui::CDisplayWindow3D::Ptr win3D_; 00224 00225 std::vector<CObservation2DRangeScan::Ptr> lst_current_laser_scans; 00226 bool isObsBasedRawlog; 00227 bool SHOW_PROGRESS_3D_REAL_TIME; 00228 int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS; 00229 bool SHOW_LASER_SCANS_3D; 00230 bool CAMERA_3DSCENE_FOLLOWS_ROBOT; 00231 }; 00232