20 #include <std_msgs/String.h> 21 #include <std_msgs/Int32.h> 22 #include <geometry_msgs/PoseArray.h> 23 #include <geometry_msgs/PoseWithCovarianceStamped.h> 24 #include <visualization_msgs/MarkerArray.h> 25 #include <visualization_msgs/Marker.h> 27 #include <mrpt_bridge/pose.h> 28 #include <mrpt_bridge/landmark.h> 29 #include <mrpt_bridge/mrpt_log_macros.h> 30 #include <mrpt_bridge/time.h> 31 #include <mrpt/utils/CFileGZInputStream.h> 32 #include <mrpt/utils/CFileGZOutputStream.h> 33 #include <mrpt/utils/CConfigFile.h> 34 #include <mrpt/random.h> 36 #include <mrpt/system/filesystem.h> 37 #include <mrpt/system/os.h> 38 #include <mrpt/poses/CPosePDFGaussian.h> 39 #include <mrpt/poses/CPose3DPDF.h> 40 #include <mrpt/opengl/CSetOfLines.h> 41 #include <mrpt/opengl/CGridPlaneXY.h> 42 #include <mrpt/opengl/CEllipsoid.h> 43 #include <mrpt/opengl/stock_objects.h> 44 #include <mrpt_msgs/ObservationRangeBearing.h> 46 #if MRPT_VERSION >= 0x130 47 #include <mrpt/obs/CObservationOdometry.h> 48 #include <mrpt/obs/CActionRobotMovement2D.h> 49 #include <mrpt/obs/CActionRobotMovement3D.h> 50 #include <mrpt/obs/CRawlog.h> 51 using namespace mrpt::maps;
52 using namespace mrpt::obs;
54 #include <mrpt/slam/CObservationOdometry.h> 55 #include <mrpt/slam/CActionRobotMovement2D.h> 56 #include <mrpt/slam/CActionRobotMovement3D.h> 57 #include <mrpt/slam/CRawlog.h> 86 void computeEllipseOrientationScale2D(
tf::Quaternion& orientation, Eigen::Vector2d& scale,
87 const mrpt::math::CMatrixDouble covariance);
95 void makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues);
112 bool is_file_exists(
const std::string& name);
120 void viz_dataAssociation();
127 void odometryForCallback(CObservationOdometry::Ptr& _odometry,
const std_msgs::Header& _msg_header);
138 void landmarkCallback(
const mrpt_msgs::ObservationRangeBearing& _msg);
144 void updateSensorPose(std::string _frame_id);
157 bool waitForTransform(mrpt::poses::CPose3D& des,
const std::string&
target_frame,
const std::string& source_frame,
float t_exec
the time which take one SLAM update execution
The EKFslamWrapper class provides the ROS wrapper for EKF SLAM 2d from MRPT libraries.
CTicTac tictac
timer for SLAM performance evaluation
std::string odom_frame_id
/odom frame
double rawlog_play_delay
delay of replay from rawlog file
std::string global_frame_id
/map frame
std::string base_frame_id
robot frame
ros::NodeHandle n_
Node handler.
bool rawlog_play_
true if rawlog file exists
tf::TransformListener listenerTF_
transform listener
std::string sensor_source
2D laser scans
std::string rawlog_filename
name of rawlog file
double ellipse_scale_
Scale of covariance ellipses.
std::string ini_filename
name of ini file
The EKFslam class provides EKF SLAM 2d from MRPT libraries.
std::vector< ros::Subscriber > sensorSub_
list of sensors topics
tf::TransformBroadcaster tf_broadcaster_
transform broadcaster
std::map< std::string, mrpt::poses::CPose3D > landmark_poses_
landmark poses with respect to the map
ros::Publisher state_viz_pub_
publishers