mrpt_icp_slam_2d_wrapper.h
Go to the documentation of this file.
1 /*
2  * File: mrpt_icp_slam_2d_wrapper.h
3  * Author: Vladislav Tananaev
4  *
5  */
6 
7 #pragma once
8 
9 // MRPT libraries
14 #include <mrpt/utils/CConfigFile.h>
17 #include <mrpt/system/os.h>
18 #include <mrpt/system/filesystem.h>
19 #include <mrpt/opengl/CPlanarLaserScan.h> // This class lives in the lib [mrpt-maps] and must be included by hand
21 #include <mrpt/poses/CPose3DPDF.h>
22 
24 
25 #include <stdint.h>
26 #include <iostream> // std::cout
27 #include <fstream> // std::ifstream
28 #include <string>
29 
30 // add ros libraries
31 #include <ros/ros.h>
32 #include <ros/package.h>
33 #include <tf/transform_listener.h>
35 // add ros msgs
36 #include <nav_msgs/OccupancyGrid.h>
37 #include "nav_msgs/MapMetaData.h"
38 #include <nav_msgs/Path.h>
39 #include <std_msgs/String.h>
40 #include <std_msgs/Header.h>
41 #include <std_msgs/Int32.h>
42 #include <nav_msgs/GetMap.h>
43 #include <geometry_msgs/PoseArray.h>
44 #include <geometry_msgs/PoseStamped.h>
45 #include <geometry_msgs/PoseWithCovarianceStamped.h>
46 #include <sensor_msgs/LaserScan.h>
47 #include <sensor_msgs/PointCloud.h>
48 #include <visualization_msgs/MarkerArray.h>
49 #include <visualization_msgs/Marker.h>
50 
51 // mrpt bridge libs
52 #include <mrpt_bridge/pose.h>
53 #include <mrpt_bridge/map.h>
55 #include <mrpt_bridge/laser_scan.h>
56 #include <mrpt_bridge/time.h>
58 #include <mrpt/version.h>
59 #if MRPT_VERSION >= 0x130
60 #include <mrpt/obs/CActionRobotMovement2D.h>
61 #include <mrpt/obs/CActionRobotMovement3D.h>
62 #include <mrpt/obs/CActionCollection.h>
63 #include <mrpt/obs/CObservationOdometry.h>
64 #include <mrpt/obs/CSensoryFrame.h>
65 #include <mrpt/maps/CMultiMetricMap.h>
66 #include <mrpt/obs/CRawlog.h>
67 using namespace mrpt::maps;
68 using namespace mrpt::obs;
69 #else
70 #include <mrpt/slam/CActionRobotMovement2D.h>
71 #include <mrpt/slam/CActionRobotMovement3D.h>
72 #include <mrpt/slam/CActionCollection.h>
73 #include <mrpt/slam/CObservationOdometry.h>
74 #include <mrpt/slam/CSensoryFrame.h>
75 #include <mrpt/slam/CMultiMetricMap.h>
76 #include <mrpt/slam/CRawlog.h>
77 #endif
78 using namespace mrpt;
79 using namespace mrpt::slam;
80 using namespace mrpt::opengl;
81 using namespace mrpt::gui;
82 using namespace mrpt::system;
83 using namespace mrpt::math;
84 using namespace mrpt::utils;
85 using namespace mrpt::poses;
86 using namespace std;
87 
93 {
94 public:
99 
103  ~ICPslamWrapper();
104 
110  void read_iniFile(std::string ini_filename);
114  void init3Dwindow();
118  void run3Dwindow();
119 
123  void get_param();
127  void init();
133  bool rawlogPlay();
139  bool is_file_exists(const std::string &name);
149  void laserCallback(const sensor_msgs::LaserScan &_msg);
154  void publishTF();
159  void publishMapPose();
165  void updateSensorPose(std::string _frame_id);
171  void updateTrajectoryTimerCallback(const ros::TimerEvent& event);
177  void publishTrajectoryTimerCallback(const ros::TimerEvent& event);
178 
179 protected:
184 
185  std::string rawlog_filename;
186  std::string ini_filename;
187  std::string global_frame_id;
188  std::string odom_frame_id;
189  std::string base_frame_id;
190  geometry_msgs::PoseStamped pose;
191 
193  nav_msgs::Path path;
194 
197 
200 
201  // Sensor source
202  std::string sensor_source;
203  std::map<std::string, mrpt::poses::CPose3D> laser_poses_;
204 
205  // Subscribers
206  std::vector<ros::Subscriber> sensorSub_;
207 
209  // CPose3DPDF::Ptr curPDF; ///<current robot pose
210  ros::Publisher pub_map_, pub_metadata_, pub_pose_, pub_point_cloud_;
211 
214 
216  float t_exec;
217  CSensoryFrame::Ptr observations;
218  CObservation::Ptr observation;
220 
222 
223  mrpt::gui::CDisplayWindow3D::Ptr win3D_;
224 
225  std::vector<CObservation2DRangeScan::Ptr> lst_current_laser_scans;
231 };
232 
uint64_t TTimeStamp
mrpt::gui::CDisplayWindow3D::Ptr win3D_
MRPT window.
const CMultiMetricMap * metric_map_
receive map after iteration of SLAM to metric map
nav_msgs::Path path
trajectory path
std::string odom_frame_id
/odom frame
CSensoryFrame::Ptr observations
bool rawlog_play_
true if rawlog file exists
float t_exec
the time which take one SLAM update execution
geometry_msgs::PoseStamped pose
the robot pose
double rawlog_play_delay
delay of replay from rawlog file
std::vector< ros::Subscriber > sensorSub_
list of sensors topics
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
tf::TransformListener listenerTF_
transform listener
CMetricMapBuilderICP mapBuilder
icp slam class
CTicTac tictac
timer for SLAM performance evaluation
GLuint const GLchar * name
std::string ini_filename
name of ini file
ros::NodeHandle n_
Node Handle.
CObservation::Ptr observation
std::string global_frame_id
/map frame
ros::Time stamp
timestamp for observations
ros::Timer publish_trajectory_timer
timer for publish trajectory
The ICPslamWrapper class provides 2d icp based SLAM from MRPT libraries.
ros::Publisher trajectory_pub_
trajectory publisher
double trajectory_update_rate
trajectory update rate(Hz)
std::string rawlog_filename
name of rawlog file
mrpt::system::TTimeStamp timeLastUpdate_
last update of the pose and map
std::string base_frame_id
robot frame
std::map< std::string, mrpt::poses::CPose3D > laser_poses_
laser scan poses with respect to the map
double trajectory_publish_rate
trajectory publish rate(Hz)
ros::Timer update_trajector_timer
timer for update trajectory
std::string sensor_source
2D laser scans
std::vector< CObservation2DRangeScan::Ptr > lst_current_laser_scans


mrpt_icp_slam_2d
Author(s): Jose Luis Blanco Claraco, Vladislav Tananaev
autogenerated on Sat May 2 2020 03:44:29