odom_estimation_node.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 * 
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 /* Author: Wim Meeussen */
00035 
00036 #ifndef __ODOM_ESTIMATION_NODE__
00037 #define __ODOM_ESTIMATION_NODE__
00038 
00039 // ros stuff
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 // messages
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 // log files
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   // ekf filter
00113   OdomEstimation my_filter_;
00114 
00115   // estimated robot pose message to send
00116   geometry_msgs::PoseWithCovarianceStamped  output_; 
00117 
00118   // robot state
00119   tf::TransformListener    robot_state_;
00120   tf::TransformBroadcaster odom_broadcaster_;
00121 
00122   // vectors
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   // log files for debugging
00139   std::ofstream odom_file_, imu_file_, vo_file_, gps_file_, corr_file_, time_file_, extra_file_;
00140 
00141   // counters
00142   unsigned int odom_callback_counter_, imu_callback_counter_, vo_callback_counter_,gps_callback_counter_, ekf_sent_counter_;
00143 
00144 }; // class
00145 
00146 }; // namespace
00147 
00148 #endif


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:38