mrpt_rbpf_slam_wrapper.h
Go to the documentation of this file.
1 /*
2  * File: mrpt_rbpf_slam_wrapper.h
3  * Author: Vladislav Tananaev
4  *
5  */
6 
7 #pragma once
8 
9 #include <iostream> // std::cout
10 #include <fstream> // std::ifstream
11 #include <string>
13 
14 // add ros libraries
15 #include <ros/ros.h>
16 #include <ros/package.h>
17 #include <tf/transform_listener.h>
19 // add ros msgs
20 #include <nav_msgs/OccupancyGrid.h>
21 #include "nav_msgs/MapMetaData.h"
22 #include <std_msgs/String.h>
23 #include <std_msgs/Int32.h>
24 #include <nav_msgs/GetMap.h>
25 #include <geometry_msgs/PoseArray.h>
26 #include <geometry_msgs/PoseWithCovarianceStamped.h>
27 #include <sensor_msgs/LaserScan.h>
28 #include <visualization_msgs/MarkerArray.h>
29 #include <visualization_msgs/Marker.h>
30 // mrpt msgs
31 #include "mrpt_msgs/ObservationRangeBeacon.h"
32 // mrpt bridge libs
33 #include <mrpt_bridge/pose.h>
34 #include <mrpt_bridge/map.h>
35 #include <mrpt_bridge/mrpt_log_macros.h>
36 #include <mrpt_bridge/laser_scan.h>
37 #include <mrpt_bridge/beacon.h>
38 #include <mrpt_bridge/time.h>
39 
40 #include <mrpt/obs/CObservationBeaconRanges.h>
41 
42 namespace mrpt_rbpf_slam
43 {
49 class PFslamWrapper : public PFslam
50 {
51 public:
52  PFslamWrapper();
53  ~PFslamWrapper() = default;
54 
58  bool getParams(const ros::NodeHandle& nh_p);
59 
63  bool init(ros::NodeHandle& nh);
64 
70  bool rawlogPlay();
71 
75  void publishMapPose();
76 
87  void callbackBeacon(const mrpt_msgs::ObservationRangeBeacon& msg);
88 
99  void laserCallback(const sensor_msgs::LaserScan& msg);
100 
113  bool waitForTransform(mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame,
114  const ros::Time& time, const ros::Duration& timeout,
115  const ros::Duration& polling_sleep_duration = ros::Duration(0.01));
116 
123  void odometryForCallback(mrpt::obs::CObservationOdometry::Ptr& odometry, const std_msgs::Header& msg_header);
124 
130  void updateSensorPose(const std::string& frame_id);
131 
136  void publishTF();
137 
142  void vizBeacons();
143 
144 private:
146  bool rawlog_play_{ false };
147 
148  std::string rawlog_filename_;
149  std::string ini_filename_;
150  std::string global_frame_id_;
151  std::string odom_frame_id_;
152  std::string base_frame_id_;
153 
154  // Sensor source
155  std::string sensor_source_;
156 
157  std::map<std::string, mrpt::poses::CPose3D> laser_poses_;
158  std::map<std::string, mrpt::poses::CPose3D> beacon_poses_;
159 
160  // Subscribers
161  std::vector<ros::Subscriber> sensorSub_;
162 
163  // read rawlog file
164  std::vector<std::pair<mrpt::obs::CActionCollection, mrpt::obs::CSensoryFrame>> data_;
165 
168  std::vector<mrpt::opengl::CEllipsoid::Ptr> viz_beacons_;
169 
172 
175 #if MRPT_VERSION >= 0x199
176  mrpt::system::CTicTac tictac_;
177 #else
178  mrpt::utils::CTicTac tictac_;
179 #endif
180  float t_exec_;
181 };
182 } // namespace mrpt_rbpf_slam
std::string global_frame_id_
/map frame
tf::TransformListener listenerTF_
transform listener
void odometryForCallback(mrpt::obs::CObservationOdometry::Ptr &odometry, const std_msgs::Header &msg_header)
Get the odometry for received observation.
std::vector< ros::Subscriber > sensorSub_
list of sensors topics
The PFslam class provides Rao-Blackwellized Particle filter SLAM from MRPT libraries.
std::string rawlog_filename_
name of rawlog file
std::vector< std::pair< mrpt::obs::CActionCollection, mrpt::obs::CSensoryFrame > > data_
void laserCallback(const sensor_msgs::LaserScan &msg)
Callback function for the laser scans.
ros::Publisher beacon_viz_pub_
publishers for map and pose particles
std::string target_frame
std::string odom_frame_id_
/odom frame
void vizBeacons()
Correct visualization for ro slam.
The PFslamWrapper class provides the ROS wrapper for Rao-Blackwellized Particle filter SLAM from MRPT...
bool getParams(const ros::NodeHandle &nh_p)
Read the parameters from launch file.
float t_exec_
the time which take one SLAM update execution
void publishMapPose()
Publish beacon or grid map and robot pose.
std::map< std::string, mrpt::poses::CPose3D > beacon_poses_
beacon poses with respect to the map
bool waitForTransform(mrpt::poses::CPose3D &des, const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01))
Wait for transform between odometry frame and the robot frame.
bool init(ros::NodeHandle &nh)
Initialize publishers subscribers and RBPF slam.
void updateSensorPose(const std::string &frame_id)
Update the pose of the sensor with respect to the robot.
void callbackBeacon(const mrpt_msgs::ObservationRangeBeacon &msg)
Callback function for the beacons.
std::string sensor_source_
2D laser scans
double rawlog_play_delay_
delay of replay from rawlog file
bool rawlog_play_
true if rawlog file exists
std::map< std::string, mrpt::poses::CPose3D > laser_poses_
laser scan poses with respect to the map
std::string ini_filename_
name of ini file
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
std::vector< mrpt::opengl::CEllipsoid::Ptr > viz_beacons_
std::string base_frame_id_
robot frame


mrpt_rbpf_slam
Author(s): Vladislav Tananaev
autogenerated on Thu Jun 6 2019 19:37:56