Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef __ODOM_ESTIMATION_NODE__
00037 #define __ODOM_ESTIMATION_NODE__
00038
00039
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #include <tf/transform_listener.h>
00043 #include <tf/transform_broadcaster.h>
00044 #include "odom_estimation.h"
00045 #include <robot_pose_ekf/GetStatus.h>
00046
00047
00048 #include "nav_msgs/Odometry.h"
00049 #include "geometry_msgs/Twist.h"
00050 #include "sensor_msgs/Imu.h"
00051 #include "geometry_msgs/PoseStamped.h"
00052 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00053
00054 #include <boost/thread/mutex.hpp>
00055
00056
00057 #include <fstream>
00058
00059 namespace estimation
00060 {
00061
00071 typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
00072 typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
00073 typedef boost::shared_ptr<nav_msgs::Odometry const> VoConstPtr;
00074 typedef boost::shared_ptr<nav_msgs::Odometry const> GpsConstPtr;
00075 typedef boost::shared_ptr<geometry_msgs::Twist const> VelConstPtr;
00076
00077 class OdomEstimationNode
00078 {
00079 public:
00081 OdomEstimationNode();
00082
00084 virtual ~OdomEstimationNode();
00085
00086 private:
00088 void spin(const ros::TimerEvent& e);
00089
00091 void odomCallback(const OdomConstPtr& odom);
00092
00094 void imuCallback(const ImuConstPtr& imu);
00095
00097 void voCallback(const VoConstPtr& vo);
00098
00100 void gpsCallback(const GpsConstPtr& gps);
00101
00102
00104 bool getStatus(robot_pose_ekf::GetStatus::Request& req, robot_pose_ekf::GetStatus::Response& resp);
00105
00106 ros::NodeHandle node_;
00107 ros::Timer timer_;
00108 ros::Publisher pose_pub_;
00109 ros::Subscriber odom_sub_, imu_sub_, vo_sub_,gps_sub_;
00110 ros::ServiceServer state_srv_;
00111
00112
00113 OdomEstimation my_filter_;
00114
00115
00116 geometry_msgs::PoseWithCovarianceStamped output_;
00117
00118
00119 tf::TransformListener robot_state_;
00120 tf::TransformBroadcaster odom_broadcaster_;
00121
00122
00123 tf::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_;
00124 tf::Transform base_vo_init_;
00125 tf::Transform base_gps_init_;
00126 tf::StampedTransform camera_base_;
00127 ros::Time odom_time_, imu_time_, vo_time_, gps_time_;
00128 ros::Time odom_stamp_, imu_stamp_, vo_stamp_, gps_stamp_, filter_stamp_;
00129 ros::Time odom_init_stamp_, imu_init_stamp_, vo_init_stamp_, gps_init_stamp_;
00130 bool odom_active_, imu_active_, vo_active_, gps_active_;
00131 bool odom_used_, imu_used_, vo_used_, gps_used_;
00132 bool odom_initializing_, imu_initializing_, vo_initializing_, gps_initializing_;
00133 double timeout_;
00134 MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_;
00135 bool debug_, self_diagnose_;
00136 std::string output_frame_, base_footprint_frame_, tf_prefix_;
00137
00138
00139 std::ofstream odom_file_, imu_file_, vo_file_, gps_file_, corr_file_, time_file_, extra_file_;
00140
00141
00142 unsigned int odom_callback_counter_, imu_callback_counter_, vo_callback_counter_,gps_callback_counter_, ekf_sent_counter_;
00143
00144 };
00145
00146 };
00147
00148 #endif