Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007 #pragma once
00008 #include <iostream>
00009 #include <fstream>
00010 #include <string>
00011 #include "mrpt_ekf_slam_2d/mrpt_ekf_slam_2d.h"
00012
00013
00014 #include <ros/ros.h>
00015 #include <ros/package.h>
00016 #include <tf/transform_listener.h>
00017 #include <tf/transform_broadcaster.h>
00018
00019
00020 #include <std_msgs/String.h>
00021 #include <std_msgs/Int32.h>
00022 #include <geometry_msgs/PoseArray.h>
00023 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00024 #include <visualization_msgs/MarkerArray.h>
00025 #include <visualization_msgs/Marker.h>
00026
00027 #include <mrpt_bridge/pose.h>
00028 #include <mrpt_bridge/landmark.h>
00029 #include <mrpt_bridge/mrpt_log_macros.h>
00030 #include <mrpt_bridge/time.h>
00031 #include <mrpt/utils/CFileGZInputStream.h>
00032 #include <mrpt/utils/CFileGZOutputStream.h>
00033 #include <mrpt/utils/CConfigFile.h>
00034 #include <mrpt/random.h>
00035
00036 #include <mrpt/system/filesystem.h>
00037 #include <mrpt/system/os.h>
00038 #include <mrpt/poses/CPosePDFGaussian.h>
00039 #include <mrpt/poses/CPose3DPDF.h>
00040 #include <mrpt/opengl/CSetOfLines.h>
00041 #include <mrpt/opengl/CGridPlaneXY.h>
00042 #include <mrpt/opengl/CEllipsoid.h>
00043 #include <mrpt/opengl/stock_objects.h>
00044 #include <mrpt_msgs/ObservationRangeBearing.h>
00045
00046 #if MRPT_VERSION >= 0x130
00047 #include <mrpt/obs/CObservationOdometry.h>
00048 #include <mrpt/obs/CActionRobotMovement2D.h>
00049 #include <mrpt/obs/CActionRobotMovement3D.h>
00050 #include <mrpt/obs/CRawlog.h>
00051 using namespace mrpt::maps;
00052 using namespace mrpt::obs;
00053 #else
00054 #include <mrpt/slam/CObservationOdometry.h>
00055 #include <mrpt/slam/CActionRobotMovement2D.h>
00056 #include <mrpt/slam/CActionRobotMovement3D.h>
00057 #include <mrpt/slam/CRawlog.h>
00058 #endif
00059
00064 class EKFslamWrapper : EKFslam
00065 {
00066 public:
00070 EKFslamWrapper();
00074 ~EKFslamWrapper();
00078 void get_param();
00086 void computeEllipseOrientationScale2D(tf::Quaternion& orientation, Eigen::Vector2d& scale,
00087 const mrpt::math::CMatrixDouble covariance);
00095 void makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues);
00099 void init();
00105 bool rawlogPlay();
00106
00112 bool is_file_exists(const std::string& name);
00116 void viz_state();
00120 void viz_dataAssociation();
00127 void odometryForCallback(CObservationOdometry::Ptr& _odometry, const std_msgs::Header& _msg_header);
00138 void landmarkCallback(const mrpt_msgs::ObservationRangeBearing& _msg);
00144 void updateSensorPose(std::string _frame_id);
00157 bool waitForTransform(mrpt::poses::CPose3D& des, const std::string& target_frame, const std::string& source_frame,
00158 const ros::Time& time, const ros::Duration& timeout,
00159 const ros::Duration& polling_sleep_duration = ros::Duration(0.01));
00160
00165 void publishTF();
00166
00167 private:
00168 ros::NodeHandle n_;
00169 double rawlog_play_delay;
00170 double ellipse_scale_;
00171 bool rawlog_play_;
00172
00173 std::vector<ros::Subscriber> sensorSub_;
00174 std::string rawlog_filename;
00175 std::string ini_filename;
00176 std::string global_frame_id;
00177 std::string odom_frame_id;
00178 std::string base_frame_id;
00179
00180
00181 std::string sensor_source;
00182
00183 std::map<std::string, mrpt::poses::CPose3D> landmark_poses_;
00184
00185 CTicTac tictac;
00186 float t_exec;
00187
00188 ros::Publisher data_association_viz_pub_, state_viz_pub_;
00189 tf::TransformListener listenerTF_;
00190 tf::TransformBroadcaster tf_broadcaster_;
00191 };
00192