10 #include <mrpt/slam/CMetricMapBuilderICP.h> 11 #include <mrpt/opengl/COpenGLScene.h> 12 #include <mrpt/opengl/CGridPlaneXY.h> 13 #include <mrpt/opengl/stock_objects.h> 14 #include <mrpt/utils/CConfigFile.h> 15 #include <mrpt/utils/CFileGZInputStream.h> 16 #include <mrpt/utils/CFileGZOutputStream.h> 17 #include <mrpt/system/os.h> 18 #include <mrpt/system/filesystem.h> 19 #include <mrpt/opengl/CPlanarLaserScan.h> 20 #include <mrpt/poses/CPosePDFGaussian.h> 21 #include <mrpt/poses/CPose3DPDF.h> 23 #include <mrpt/gui/CDisplayWindow3D.h> 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> 52 #include <mrpt_bridge/pose.h> 53 #include <mrpt_bridge/map.h> 54 #include <mrpt_bridge/mrpt_log_macros.h> 55 #include <mrpt_bridge/laser_scan.h> 56 #include <mrpt_bridge/time.h> 57 #include <mrpt_bridge/point_cloud.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;
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> 110 void read_iniFile(std::string ini_filename);
139 bool is_file_exists(
const std::string &name);
149 void laserCallback(
const sensor_msgs::LaserScan &_msg);
159 void publishMapPose();
165 void updateSensorPose(std::string _frame_id);
190 geometry_msgs::PoseStamped
pose;
bool CAMERA_3DSCENE_FOLLOWS_ROBOT
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 SHOW_PROGRESS_3D_REAL_TIME
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
int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
tf::TransformListener listenerTF_
transform listener
CMetricMapBuilderICP mapBuilder
icp slam class
CTicTac tictac
timer for SLAM performance evaluation
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