├── .github └── workflows │ └── manual.yml ├── 3-Clause_BSD_license ├── CHANGELOG.rst ├── CMakeLists.txt ├── CODEOWNERS ├── Images └── Output.png ├── README.md ├── example_with_gps.launch ├── include └── robot_pose_ekf │ ├── nonlinearanalyticconditionalgaussianodo.h │ ├── odom_estimation.h │ └── odom_estimation_node.h ├── package.xml ├── plotekf.m ├── robot_pose_ekf.launch ├── scripts └── wtf.py ├── src ├── nonlinearanalyticconditionalgaussianodo.cpp ├── odom_estimation.cpp └── odom_estimation_node.cpp ├── srv └── GetStatus.srv └── test ├── test_robot_pose_ekf.cpp ├── test_robot_pose_ekf.launch ├── test_robot_pose_ekf_zero_covariance.cpp └── test_robot_pose_ekf_zero_covariance.launch /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /3-Clause_BSD_license: -------------------------------------------------------------------------------- 1 | Copyright 2020, Wim Meeussen. Maintained by ROS Orphaned Package Maintainers 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package robot_pose_ekf 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.14.2 (2017-08-14) 6 | ------------------- 7 | 8 | 1.14.1 (2017-08-07) 9 | ------------------- 10 | * Fix CMakeLists + package.xmls (`#548 `_) 11 | * Initialization of filter with GPS and odometry. 12 | * Contributors: Martin Günther, Vincent Rabaud, azaganidis 13 | 14 | 1.14.0 (2016-05-20) 15 | ------------------- 16 | * add to install script/ directory 17 | * add execute bit to scripts/wtf.py 18 | * robot_pose_ekf/src/odom_estimation_node.cpp: add to print gps sensor and used/not used in getStatus 19 | * Contributors: Kei Okada 20 | 21 | 1.13.1 (2015-10-29) 22 | ------------------- 23 | 24 | 1.13.0 (2015-03-17) 25 | ------------------- 26 | 27 | 1.12.0 (2015-02-04) 28 | ------------------- 29 | * update maintainer email 30 | * Contributors: Michael Ferguson 31 | 32 | 1.11.15 (2015-02-03) 33 | -------------------- 34 | * Fix bfl includes in robot_pose_ekf 35 | * Contributors: Jochen Sprickerhof 36 | 37 | 1.11.14 (2014-12-05) 38 | -------------------- 39 | 40 | 1.11.13 (2014-10-02) 41 | -------------------- 42 | 43 | 1.11.12 (2014-10-01) 44 | -------------------- 45 | * Fix EKF topic name so that it is unaffected by tf_prefix 46 | * Install launch files, closes `#249 `_ 47 | * Fixed hardcoded tf frames issue by fetching base_footprint_frame_ value from param server and using it in OdomEstimation filter 48 | * Fixed hardcoded tf frames issue by adding variables for output and base_footprint frames along with mutator methods 49 | * Contributors: Jochen Sprickerhof, Michael Ferguson, Murilo FM 50 | 51 | 1.11.11 (2014-07-23) 52 | -------------------- 53 | 54 | 1.11.10 (2014-06-25) 55 | -------------------- 56 | 57 | 1.11.9 (2014-06-10) 58 | ------------------- 59 | * fix robot_pose_ekf test 60 | * Contributors: Michael Ferguson 61 | 62 | 1.11.8 (2014-05-21) 63 | ------------------- 64 | * fix build, was broken by `#175 `_ 65 | * Contributors: Michael Ferguson 66 | 67 | 1.11.7 (2014-05-21) 68 | ------------------- 69 | * Even longer Time limit for EKF Test 70 | * make rostest in CMakeLists optional 71 | * Contributors: David Lu!!, Lukas Bulwahn 72 | 73 | 1.11.5 (2014-01-30) 74 | ------------------- 75 | * check for CATKIN_ENABLE_TESTING 76 | * Download test data from download.ros.org instead of willow 77 | * Change maintainer from Hersh to Lu 78 | 79 | 1.11.4 (2013-09-27) 80 | ------------------- 81 | * Package URL Updates 82 | * upgrade depracated download data calls. 83 | * use tf_prefix to lookup and send transforms 84 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robot_pose_ekf) 3 | 4 | # bfl (Bayesian Filtering Library) is a third party package that uses pkg-config 5 | find_package(PkgConfig) 6 | pkg_check_modules(BFL REQUIRED orocos-bfl) 7 | 8 | include_directories(${BFL_INCLUDE_DIRS}) 9 | link_directories(${BFL_LIBRARY_DIRS}) 10 | 11 | find_package(catkin REQUIRED 12 | COMPONENTS 13 | roscpp 14 | tf 15 | nav_msgs 16 | std_msgs 17 | geometry_msgs 18 | sensor_msgs 19 | message_generation 20 | ) 21 | 22 | find_package(Boost REQUIRED COMPONENTS thread) 23 | 24 | # services 25 | add_service_files( 26 | DIRECTORY srv 27 | FILES 28 | GetStatus.srv 29 | ) 30 | 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs 34 | ) 35 | 36 | catkin_package( 37 | CATKIN_DEPENDS 38 | geometry_msgs 39 | message_runtime 40 | nav_msgs 41 | roscpp 42 | sensor_msgs 43 | std_msgs 44 | ) 45 | 46 | include_directories( 47 | "include" 48 | ${catkin_INCLUDE_DIRS} 49 | ${Boost_INCLUDE_DIRS} 50 | ) 51 | 52 | add_executable(robot_pose_ekf 53 | src/odom_estimation.cpp 54 | src/nonlinearanalyticconditionalgaussianodo.cpp 55 | src/odom_estimation_node.cpp) 56 | target_link_libraries(robot_pose_ekf 57 | ${catkin_LIBRARIES} 58 | ${Boost_LIBRARIES} 59 | ${BFL_LIBRARIES} 60 | ) 61 | add_dependencies(robot_pose_ekf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 62 | 63 | install( 64 | TARGETS 65 | robot_pose_ekf 66 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 67 | ) 68 | 69 | install( 70 | FILES robot_pose_ekf.launch example_with_gps.launch 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 72 | ) 73 | 74 | install( 75 | PROGRAMS scripts/wtf.py 76 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 77 | ) 78 | 79 | ## Tests are failing on OSX for an unknown reason 80 | include(CMakeDetermineSystem) 81 | if(CMAKE_SYSTEM_NAME MATCHES "Linux") 82 | if(CATKIN_ENABLE_TESTING) 83 | 84 | catkin_download_test_data( 85 | download_data_ekf_test2_indexed.bag 86 | http://download.ros.org/data/robot_pose_ekf/ekf_test2_indexed.bag 87 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 88 | MD5 71addef0ed900e05b301e0b4fdca99e2 89 | ) 90 | add_executable(test_robot_pose_ekf test/test_robot_pose_ekf.cpp) 91 | target_link_libraries(test_robot_pose_ekf 92 | ${Boost_LIBRARIES} 93 | ${catkin_LIBRARIES} 94 | ${BFL_LIBRARIES} 95 | gtest 96 | ) 97 | add_dependencies(test_robot_pose_ekf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 98 | 99 | catkin_download_test_data( 100 | download_data_zero_covariance.bag 101 | http://download.ros.org/data/robot_pose_ekf/zero_covariance_indexed.bag 102 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test 103 | MD5 1f1f4e361a9e0b0f6b1379b2dd011088 104 | ) 105 | add_executable(test_robot_pose_ekf_zero_covariance test/test_robot_pose_ekf_zero_covariance.cpp) 106 | target_link_libraries(test_robot_pose_ekf_zero_covariance 107 | ${Boost_LIBRARIES} 108 | ${catkin_LIBRARIES} 109 | ${BFL_LIBRARIES} 110 | gtest 111 | ) 112 | add_dependencies(test_robot_pose_ekf_zero_covariance ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 113 | 114 | # This has to be done after we've already built targets, or catkin variables get borked 115 | find_package(rostest REQUIRED) 116 | add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_robot_pose_ekf.launch) 117 | add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_robot_pose_ekf_zero_covariance.launch) 118 | 119 | endif(CATKIN_ENABLE_TESTING) 120 | endif(CMAKE_SYSTEM_NAME MATCHES "Linux") 121 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /Images/Output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/robot_pose_ekf/6bdf918f7ede7a4bf96699da9e9e5422b31fd07e/Images/Output.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Udacity - Robotics NanoDegree Program](https://s3-us-west-1.amazonaws.com/udacity-robotics/Extra+Images/RoboND_flag.png)](https://www.udacity.com/robotics) 2 | 3 | # robot_pose_ekf package 4 | The [robot_pose_ekf](http://wiki.ros.org/robot_pose_ekf) ROS package applies sensor fusion on the robot IMU and odometry values to estimate its 3D pose. 5 | 6 | ### Nodes 7 | The package contains a single node 8 | 1. **robot_pose_ekf**: Implements an Extended Kalman Filter, subscribes to robot measurements, and publishes a filtered 3D pose. 9 | * Script File: wtf.py 10 | * Subscriber: "/odom", "/imu_data", and "/vo " 11 | * Publisher: "/robot_pose_ekf/odom_combined" 12 | 13 | ![alt text](Images/Output.png) 14 | 15 | ### Steps to launch the nodes 16 | #### Step1: Install the package 17 | ```sh 18 | $ cd /home/workspace/catkin_ws/src/ 19 | $ git clone https://github.com/udacity/robot_pose_ekf 20 | ``` 21 | #### Step2: Edit the robot_pose_ekf.launch file 22 | ```html 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | ``` 40 | #### Step3: Build the package 41 | ``` 42 | $ cd /home/workspace/catkin_ws 43 | ``` 44 | ``` 45 | $ catkin_make 46 | ``` 47 | ``` 48 | $ source devel/setup.bash 49 | ``` 50 | #### Step4: Launch the node 51 | ``` 52 | $ roslaunch robot_pose_ekf robot_pose_ekf.launch 53 | ``` 54 | -------------------------------------------------------------------------------- /example_with_gps.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /include/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2008 Wim Meeussen 2 | // 3 | // This program is free software; you can redistribute it and/or modify 4 | // it under the terms of the GNU Lesser General Public License as published by 5 | // the Free Software Foundation; either version 2.1 of the License, or 6 | // (at your option) any later version. 7 | // 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU Lesser General Public License for more details. 12 | // 13 | // You should have received a copy of the GNU Lesser General Public License 14 | // along with this program; if not, write to the Free Software 15 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 16 | // 17 | 18 | 19 | #ifndef __NON_LINEAR_SYSTEM_CONDITIONAL_GAUSSIAN_ODO__ 20 | #define __NON_LINEAR_SYSTEM_CONDITIONAL_GAUSSIAN_ODO__ 21 | 22 | #include 23 | 24 | namespace BFL 25 | { 26 | /// Non Linear Conditional Gaussian 27 | /** 28 | - \f$ \mu = Matrix[1] . ConditionalArguments[0] + 29 | Matrix[2]. ConditionalArguments[1] + ... + Noise.\mu \f$ 30 | - Covariance is independent of the ConditionalArguments, and is 31 | the covariance of the Noise pdf 32 | */ 33 | class NonLinearAnalyticConditionalGaussianOdo : public AnalyticConditionalGaussianAdditiveNoise 34 | { 35 | public: 36 | /// Constructor 37 | /** @pre: Every Matrix should have the same amount of rows! 38 | This is currently not checked. The same goes for the number 39 | of columns, which should be equal to the number of rows of 40 | the corresponding conditional argument! 41 | @param additiveNoise Pdf representing the additive Gaussian uncertainty 42 | */ 43 | NonLinearAnalyticConditionalGaussianOdo( const Gaussian& additiveNoise); 44 | 45 | /// Destructor 46 | virtual ~NonLinearAnalyticConditionalGaussianOdo(); 47 | 48 | // redefine virtual functions 49 | virtual MatrixWrapper::ColumnVector ExpectedValueGet() const; 50 | virtual MatrixWrapper::Matrix dfGet(unsigned int i) const; 51 | 52 | private: 53 | mutable MatrixWrapper::Matrix df; 54 | }; 55 | 56 | } // End namespace BFL 57 | 58 | #endif // 59 | -------------------------------------------------------------------------------- /include/robot_pose_ekf/odom_estimation.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * * Neither the name of the Willow Garage nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************/ 33 | 34 | /* Author: Wim Meeussen */ 35 | 36 | #ifndef __ODOM_ESTIMATION__ 37 | #define __ODOM_ESTIMATION__ 38 | 39 | // bayesian filtering 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include "nonlinearanalyticconditionalgaussianodo.h" 47 | 48 | // TF 49 | #include 50 | 51 | // msgs 52 | #include 53 | 54 | // log files 55 | #include 56 | 57 | namespace estimation 58 | { 59 | 60 | class OdomEstimation 61 | { 62 | public: 63 | /// constructor 64 | OdomEstimation(); 65 | 66 | /// destructor 67 | virtual ~OdomEstimation(); 68 | 69 | /** update the extended Kalman filter 70 | * \param odom_active specifies if the odometry sensor is active or not 71 | * \param imu_active specifies if the imu sensor is active or not 72 | * \param gps_active specifies if the gps sensor is active or not 73 | * \param vo_active specifies if the vo sensor is active or not 74 | * \param filter_time update the ekf up to this time 75 | * \param diagnostics_res returns false if the diagnostics found that the sensor measurements are inconsistent 76 | * returns true on successfull update 77 | */ 78 | bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time& filter_time, bool& diagnostics_res); 79 | 80 | /** initialize the extended Kalman filter 81 | * \param prior the prior robot pose 82 | * \param time the initial time of the ekf 83 | */ 84 | void initialize(const tf::Transform& prior, const ros::Time& time); 85 | 86 | /** check if the filter is initialized 87 | * returns true if the ekf has been initialized already 88 | */ 89 | bool isInitialized() {return filter_initialized_;}; 90 | 91 | /** get the filter posterior 92 | * \param estimate the filter posterior as a columnvector 93 | */ 94 | void getEstimate(MatrixWrapper::ColumnVector& estimate); 95 | 96 | /** get the filter posterior 97 | * \param time the time of the filter posterior 98 | * \param estimate the filter posterior as a tf transform 99 | */ 100 | void getEstimate(ros::Time time, tf::Transform& estimate); 101 | 102 | /** get the filter posterior 103 | * \param time the time of the filter posterior 104 | * \param estimate the filter posterior as a stamped tf transform 105 | */ 106 | void getEstimate(ros::Time time, tf::StampedTransform& estimate); 107 | 108 | /** get the filter posterior 109 | * \param estimate the filter posterior as a pose with covariance 110 | */ 111 | void getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate); 112 | 113 | /** Add a sensor measurement to the measurement buffer 114 | * \param meas the measurement to add 115 | */ 116 | void addMeasurement(const tf::StampedTransform& meas); 117 | 118 | /** Add a sensor measurement to the measurement buffer 119 | * \param meas the measurement to add 120 | * \param covar the 6x6 covariance matrix of this measurement, as defined in the PoseWithCovariance message 121 | */ 122 | void addMeasurement(const tf::StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar); 123 | 124 | /** set the output frame used by tf 125 | * \param output_frame the desired output frame published on /tf (e.g., odom_combined) 126 | */ 127 | void setOutputFrame(const std::string& output_frame); 128 | 129 | /** set the base_footprint frame of the robot used by tf 130 | * \param base_frame the desired base frame from which to transform when publishing the combined odometry frame (e.g., base_footprint) 131 | */ 132 | void setBaseFootprintFrame(const std::string& base_frame); 133 | 134 | private: 135 | /// correct for angle overflow 136 | void angleOverflowCorrect(double& a, double ref); 137 | 138 | // decompose Transform into x,y,z,Rx,Ry,Rz 139 | void decomposeTransform(const tf::StampedTransform& trans, 140 | double& x, double& y, double&z, double&Rx, double& Ry, double& Rz); 141 | void decomposeTransform(const tf::Transform& trans, 142 | double& x, double& y, double&z, double&Rx, double& Ry, double& Rz); 143 | 144 | 145 | // pdf / model / filter 146 | BFL::AnalyticSystemModelGaussianUncertainty* sys_model_; 147 | BFL::NonLinearAnalyticConditionalGaussianOdo* sys_pdf_; 148 | BFL::LinearAnalyticConditionalGaussian* odom_meas_pdf_; 149 | BFL::LinearAnalyticMeasurementModelGaussianUncertainty* odom_meas_model_; 150 | BFL::LinearAnalyticConditionalGaussian* imu_meas_pdf_; 151 | BFL::LinearAnalyticMeasurementModelGaussianUncertainty* imu_meas_model_; 152 | BFL::LinearAnalyticConditionalGaussian* vo_meas_pdf_; 153 | BFL::LinearAnalyticMeasurementModelGaussianUncertainty* vo_meas_model_; 154 | BFL::LinearAnalyticConditionalGaussian* gps_meas_pdf_; 155 | BFL::LinearAnalyticMeasurementModelGaussianUncertainty* gps_meas_model_; 156 | BFL::Gaussian* prior_; 157 | BFL::ExtendedKalmanFilter* filter_; 158 | MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_; 159 | 160 | // vars 161 | MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_; 162 | tf::Transform filter_estimate_old_; 163 | tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_; 164 | ros::Time filter_time_old_; 165 | bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_, gps_initialized_; 166 | 167 | // diagnostics 168 | double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_; 169 | 170 | // tf transformer 171 | tf::Transformer transformer_; 172 | 173 | std::string output_frame_; 174 | std::string base_footprint_frame_; 175 | 176 | }; // class 177 | 178 | }; // namespace 179 | 180 | #endif 181 | -------------------------------------------------------------------------------- /include/robot_pose_ekf/odom_estimation_node.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above 13 | * copyright notice, this list of conditions and the following 14 | * disclaimer in the documentation and/or other materials provided 15 | * with the distribution. 16 | * * Neither the name of the Willow Garage nor the names of its 17 | * contributors may be used to endorse or promote products derived 18 | * from this software without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************/ 33 | 34 | /* Author: Wim Meeussen */ 35 | 36 | #ifndef __ODOM_ESTIMATION_NODE__ 37 | #define __ODOM_ESTIMATION_NODE__ 38 | 39 | // ros stuff 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include "odom_estimation.h" 45 | #include 46 | 47 | // messages 48 | #include "nav_msgs/Odometry.h" 49 | #include "geometry_msgs/Twist.h" 50 | #include "sensor_msgs/Imu.h" 51 | #include "geometry_msgs/PoseStamped.h" 52 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 53 | 54 | #include 55 | 56 | // log files 57 | #include 58 | 59 | namespace estimation 60 | { 61 | 62 | /** \mainpage 63 | * \htmlinclude manifest.html 64 | * 65 | * Package Summary 66 | * This package provides two main classes: 67 | * 1) OdomEstimation performs all sensor fusion operations, and 68 | * 2) OdomEstimationNode provides a ROS wrapper around OdomEstimation 69 | */ 70 | 71 | typedef boost::shared_ptr OdomConstPtr; 72 | typedef boost::shared_ptr ImuConstPtr; 73 | typedef boost::shared_ptr VoConstPtr; 74 | typedef boost::shared_ptr GpsConstPtr; 75 | typedef boost::shared_ptr VelConstPtr; 76 | 77 | class OdomEstimationNode 78 | { 79 | public: 80 | /// constructor 81 | OdomEstimationNode(); 82 | 83 | /// destructor 84 | virtual ~OdomEstimationNode(); 85 | 86 | private: 87 | /// the mail filter loop that will be called periodically 88 | void spin(const ros::TimerEvent& e); 89 | 90 | /// callback function for odo data 91 | void odomCallback(const OdomConstPtr& odom); 92 | 93 | /// callback function for imu data 94 | void imuCallback(const ImuConstPtr& imu); 95 | 96 | /// callback function for vo data 97 | void voCallback(const VoConstPtr& vo); 98 | 99 | /// callback function for vo data 100 | void gpsCallback(const GpsConstPtr& gps); 101 | 102 | 103 | /// get the status of the filter 104 | bool getStatus(robot_pose_ekf::GetStatus::Request& req, robot_pose_ekf::GetStatus::Response& resp); 105 | 106 | ros::NodeHandle node_; 107 | ros::Timer timer_; 108 | ros::Publisher pose_pub_; 109 | ros::Subscriber odom_sub_, imu_sub_, vo_sub_,gps_sub_; 110 | ros::ServiceServer state_srv_; 111 | 112 | // ekf filter 113 | OdomEstimation my_filter_; 114 | 115 | // estimated robot pose message to send 116 | geometry_msgs::PoseWithCovarianceStamped output_; 117 | 118 | // robot state 119 | tf::TransformListener robot_state_; 120 | tf::TransformBroadcaster odom_broadcaster_; 121 | 122 | // vectors 123 | tf::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_; 124 | tf::Transform base_vo_init_; 125 | tf::Transform base_gps_init_; 126 | tf::StampedTransform camera_base_; 127 | ros::Time odom_time_, imu_time_, vo_time_, gps_time_; 128 | ros::Time odom_stamp_, imu_stamp_, vo_stamp_, gps_stamp_, filter_stamp_; 129 | ros::Time odom_init_stamp_, imu_init_stamp_, vo_init_stamp_, gps_init_stamp_; 130 | bool odom_active_, imu_active_, vo_active_, gps_active_; 131 | bool odom_used_, imu_used_, vo_used_, gps_used_; 132 | bool odom_initializing_, imu_initializing_, vo_initializing_, gps_initializing_; 133 | double timeout_; 134 | MatrixWrapper::SymmetricMatrix odom_covariance_, imu_covariance_, vo_covariance_, gps_covariance_; 135 | bool debug_, self_diagnose_; 136 | std::string output_frame_, base_footprint_frame_, tf_prefix_; 137 | 138 | // log files for debugging 139 | std::ofstream odom_file_, imu_file_, vo_file_, gps_file_, corr_file_, time_file_, extra_file_; 140 | 141 | // counters 142 | unsigned int odom_callback_counter_, imu_callback_counter_, vo_callback_counter_,gps_callback_counter_, ekf_sent_counter_; 143 | 144 | }; // class 145 | 146 | }; // namespace 147 | 148 | #endif 149 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | robot_pose_ekf 3 | 1.14.2 4 | 5 | 6 | The Robot Pose EKF package is used to estimate the 3D pose of a robot, based on (partial) pose measurements coming from different sources. It uses an extended Kalman filter with a 6D model (3D position and 3D orientation) to combine measurements from wheel odometry, IMU sensor and visual odometry. The basic idea is to offer loosely coupled integration with different sensors, where sensor signals are received as ROS messages. 7 | 8 | 9 | Wim Meeussen 10 | contradict@gmail.com 11 | David V. Lu!! 12 | Michael Ferguson 13 | BSD 14 | http://wiki.ros.org/robot_pose_ekf 15 | 16 | catkin 17 | 18 | message_generation 19 | roscpp 20 | bfl 21 | std_msgs 22 | geometry_msgs 23 | sensor_msgs 24 | nav_msgs 25 | tf 26 | 27 | message_runtime 28 | roscpp 29 | bfl 30 | std_msgs 31 | geometry_msgs 32 | sensor_msgs 33 | nav_msgs 34 | tf 35 | 36 | rosbag 37 | rostest 38 | 39 | -------------------------------------------------------------------------------- /plotekf.m: -------------------------------------------------------------------------------- 1 | load /tmp/odom_file.txt; 2 | %load vo_file.txt; 3 | load /tmp/corr_file.txt; 4 | load /tmp/gps_file.txt; 5 | load /tmp/imu_file.txt; 6 | 7 | figure; 8 | hold on; 9 | axis equal; 10 | plot(odom_file(:,2), odom_file(:,3),'b'); 11 | %plot(vo_file(:,2),vo_file(:,3),'g'); 12 | plot(corr_file(:,2), corr_file(:,3),'r'); 13 | plot(gps_file(:,2), gps_file(:,3),'k'); 14 | legend('Wheel Odometry', 'Filter output', 'GPS Measurements'); 15 | %legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements'); 16 | %legend('Wheel Odometry','Visual Odometry', 'Filter output'); 17 | hold off; 18 | 19 | figure; 20 | subplot(3,1,1) 21 | hold on; 22 | plot(odom_file(:,1),odom_file(:,2),'b'); 23 | %plot(vo_file(:,1),vo_file(:,2),'g'); 24 | plot(corr_file(:,1),corr_file(:,2),'r'); 25 | plot(gps_file(:,1), gps_file(:,2), 'k'); 26 | legend('Wheel Odometry', 'Filter output', 'GPS Measurements'); 27 | %legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements'); 28 | %legend('Wheel Odometry','Visual Odometry', 'Filter output'); 29 | subplot(3,1,2) 30 | hold on; 31 | plot(odom_file(:,1),odom_file(:,3),'b'); 32 | %plot(vo_file(:,1),vo_file(:,3),'g'); 33 | plot(corr_file(:,1),corr_file(:,3),'r'); 34 | plot(gps_file(:,1), gps_file(:,3), 'k'); 35 | legend('Wheel Odometry', 'Filter output', 'GPS Measurements'); 36 | %legend('Wheel Odometry','Visual Odometry', 'Filter output', 'GPS Measurements'); 37 | %legend('Wheel Odometry','Visual Odometry', 'Filter output'); 38 | 39 | subplot(3,1,3) 40 | hold on; 41 | plot(odom_file(:,1),odom_file(:,4),'b'); 42 | %plot(vo_file(:,1),vo_file(:,7),'g'); 43 | plot(corr_file(:,1),corr_file(:,7),'r'); 44 | plot(imu_file(:,1), imu_file(:,2), 'k'); 45 | legend('Wheel Odometry', 'Filter output', 'IMU Measurements'); 46 | %legend('Wheel Odometry','Visual Odometry', 'Filter output', 'IMU Measurements'); 47 | 48 | 49 | 50 | error_odom = sqrt( (odom_file(1,2)-odom_file(end,2))^2 + (odom_file(1,3)-odom_file(end,3))^2 ) 51 | %error_vo = sqrt( (vo_file(1,2)-vo_file(end,2))^2 + (vo_file(1,3)-vo_file(end,3))^2 ) 52 | error_corr = sqrt( (corr_file(1,2)-corr_file(end,2))^2 + (corr_file(1,3)-corr_file(end,3))^2 ) 53 | error_gps = sqrt( (gps_file(1,2)-gps_file(end,2))^2 + (gps_file(1,3)-gps_file(end,3))^2 ) 54 | -------------------------------------------------------------------------------- /robot_pose_ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /scripts/wtf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from robot_pose_ekf.srv import GetStatus, GetStatusRequest 5 | 6 | if __name__ == '__main__': 7 | rospy.init_node('spawner', anonymous=True) 8 | print 'looking for node robot_pose_ekf...' 9 | rospy.wait_for_service('robot_pose_ekf/get_status') 10 | 11 | s = rospy.ServiceProxy('robot_pose_ekf/get_status', GetStatus) 12 | resp = s.call(GetStatusRequest()) 13 | print resp.status 14 | -------------------------------------------------------------------------------- /src/nonlinearanalyticconditionalgaussianodo.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2008 Wim Meeussen 2 | // 3 | // This program is free software; you can redistribute it and/or modify 4 | // it under the terms of the GNU Lesser General Public License as published by 5 | // the Free Software Foundation; either version 2.1 of the License, or 6 | // (at your option) any later version. 7 | // 8 | // This program is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | // GNU Lesser General Public License for more details. 12 | // 13 | // You should have received a copy of the GNU Lesser General Public License 14 | // along with this program; if not, write to the Free Software 15 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 16 | // 17 | 18 | #include 19 | #include // Wrapper around several rng libraries 20 | #define NUMCONDARGUMENTS_MOBILE 2 21 | 22 | namespace BFL 23 | { 24 | using namespace MatrixWrapper; 25 | 26 | 27 | NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise) 28 | : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE), 29 | df(6,6) 30 | { 31 | // initialize df matrix 32 | for (unsigned int i=1; i<=6; i++){ 33 | for (unsigned int j=1; j<=6; j++){ 34 | if (i==j) df(i,j) = 1; 35 | else df(i,j) = 0; 36 | } 37 | } 38 | } 39 | 40 | 41 | NonLinearAnalyticConditionalGaussianOdo::~NonLinearAnalyticConditionalGaussianOdo(){} 42 | 43 | ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const 44 | { 45 | ColumnVector state = ConditionalArgumentGet(0); 46 | ColumnVector vel = ConditionalArgumentGet(1); 47 | state(1) += cos(state(6)) * vel(1); 48 | state(2) += sin(state(6)) * vel(1); 49 | state(6) += vel(2); 50 | return state + AdditiveNoiseMuGet(); 51 | } 52 | 53 | Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const 54 | { 55 | if (i==0)//derivative to the first conditional argument (x) 56 | { 57 | double vel_trans = ConditionalArgumentGet(1)(1); 58 | double yaw = ConditionalArgumentGet(0)(6); 59 | 60 | df(1,3)=-vel_trans*sin(yaw); 61 | df(2,3)= vel_trans*cos(yaw); 62 | 63 | return df; 64 | } 65 | else 66 | { 67 | if (i >= NumConditionalArgumentsGet()) 68 | { 69 | cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n"; 70 | exit(-BFL_ERRMISUSE); 71 | } 72 | else{ 73 | cerr << "The df is not implemented for the" < 38 | 39 | using namespace MatrixWrapper; 40 | using namespace BFL; 41 | using namespace tf; 42 | using namespace std; 43 | using namespace ros; 44 | 45 | 46 | namespace estimation 47 | { 48 | // constructor 49 | OdomEstimation::OdomEstimation(): 50 | prior_(NULL), 51 | filter_(NULL), 52 | filter_initialized_(false), 53 | odom_initialized_(false), 54 | imu_initialized_(false), 55 | vo_initialized_(false), 56 | gps_initialized_(false), 57 | output_frame_(std::string("odom_combined")), 58 | base_footprint_frame_(std::string("base_footprint")) 59 | { 60 | // create SYSTEM MODEL 61 | ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0; 62 | SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0; 63 | for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2); 64 | Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov); 65 | sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty); 66 | sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_); 67 | 68 | // create MEASUREMENT MODEL ODOM 69 | ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0; 70 | SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0; 71 | for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1; 72 | Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov); 73 | Matrix Hodom(6,6); Hodom = 0; 74 | Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1; 75 | odom_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom); 76 | odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_); 77 | 78 | // create MEASUREMENT MODEL IMU 79 | ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0; 80 | SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0; 81 | for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1; 82 | Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov); 83 | Matrix Himu(3,6); Himu = 0; 84 | Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1; 85 | imu_meas_pdf_ = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu); 86 | imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_); 87 | 88 | // create MEASUREMENT MODEL VO 89 | ColumnVector measNoiseVo_Mu(6); measNoiseVo_Mu = 0; 90 | SymmetricMatrix measNoiseVo_Cov(6); measNoiseVo_Cov = 0; 91 | for (unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1; 92 | Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov); 93 | Matrix Hvo(6,6); Hvo = 0; 94 | Hvo(1,1) = 1; Hvo(2,2) = 1; Hvo(3,3) = 1; Hvo(4,4) = 1; Hvo(5,5) = 1; Hvo(6,6) = 1; 95 | vo_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo); 96 | vo_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(vo_meas_pdf_); 97 | 98 | // create MEASUREMENT MODEL GPS 99 | ColumnVector measNoiseGps_Mu(3); measNoiseGps_Mu = 0; 100 | SymmetricMatrix measNoiseGps_Cov(3); measNoiseGps_Cov = 0; 101 | for (unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1; 102 | Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov); 103 | Matrix Hgps(3,6); Hgps = 0; 104 | Hgps(1,1) = 1; Hgps(2,2) = 1; Hgps(3,3) = 1; 105 | gps_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS); 106 | gps_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(gps_meas_pdf_); 107 | }; 108 | 109 | 110 | 111 | // destructor 112 | OdomEstimation::~OdomEstimation(){ 113 | if (filter_) delete filter_; 114 | if (prior_) delete prior_; 115 | delete odom_meas_model_; 116 | delete odom_meas_pdf_; 117 | delete imu_meas_model_; 118 | delete imu_meas_pdf_; 119 | delete vo_meas_model_; 120 | delete vo_meas_pdf_; 121 | delete gps_meas_model_; 122 | delete gps_meas_pdf_; 123 | delete sys_pdf_; 124 | delete sys_model_; 125 | }; 126 | 127 | 128 | // initialize prior density of filter 129 | void OdomEstimation::initialize(const Transform& prior, const Time& time) 130 | { 131 | // set prior of filter 132 | ColumnVector prior_Mu(6); 133 | decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6)); 134 | SymmetricMatrix prior_Cov(6); 135 | for (unsigned int i=1; i<=6; i++) { 136 | for (unsigned int j=1; j<=6; j++){ 137 | if (i==j) prior_Cov(i,j) = pow(0.001,2); 138 | else prior_Cov(i,j) = 0; 139 | } 140 | } 141 | prior_ = new Gaussian(prior_Mu,prior_Cov); 142 | filter_ = new ExtendedKalmanFilter(prior_); 143 | 144 | // remember prior 145 | addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_)); 146 | filter_estimate_old_vec_ = prior_Mu; 147 | filter_estimate_old_ = prior; 148 | filter_time_old_ = time; 149 | 150 | // filter initialized 151 | filter_initialized_ = true; 152 | } 153 | 154 | 155 | 156 | 157 | 158 | // update filter 159 | bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time& filter_time, bool& diagnostics_res) 160 | { 161 | // only update filter when it is initialized 162 | if (!filter_initialized_){ 163 | ROS_INFO("Cannot update filter when filter was not initialized first."); 164 | return false; 165 | } 166 | 167 | // only update filter for time later than current filter time 168 | double dt = (filter_time - filter_time_old_).toSec(); 169 | if (dt == 0) return false; 170 | if (dt < 0){ 171 | ROS_INFO("Will not update robot pose with time %f sec in the past.", dt); 172 | return false; 173 | } 174 | ROS_DEBUG("Update filter at time %f with dt %f", filter_time.toSec(), dt); 175 | 176 | 177 | // system update filter 178 | // -------------------- 179 | // for now only add system noise 180 | ColumnVector vel_desi(2); vel_desi = 0; 181 | filter_->Update(sys_model_, vel_desi); 182 | 183 | 184 | // process odom measurement 185 | // ------------------------ 186 | ROS_DEBUG("Process odom meas"); 187 | if (odom_active){ 188 | if (!transformer_.canTransform(base_footprint_frame_,"wheelodom", filter_time)){ 189 | ROS_ERROR("filter time older than odom message buffer"); 190 | return false; 191 | } 192 | transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_); 193 | if (odom_initialized_){ 194 | // convert absolute odom measurements to relative odom measurements in horizontal plane 195 | Transform odom_rel_frame = Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)), 196 | filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_; 197 | ColumnVector odom_rel(6); 198 | decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6)); 199 | angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6)); 200 | // update filter 201 | odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2)); 202 | 203 | ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f", 204 | odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6)); 205 | filter_->Update(odom_meas_model_, odom_rel); 206 | diagnostics_odom_rot_rel_ = odom_rel(6); 207 | } 208 | else{ 209 | odom_initialized_ = true; 210 | diagnostics_odom_rot_rel_ = 0; 211 | } 212 | odom_meas_old_ = odom_meas_; 213 | } 214 | // sensor not active 215 | else odom_initialized_ = false; 216 | 217 | 218 | // process imu measurement 219 | // ----------------------- 220 | if (imu_active){ 221 | if (!transformer_.canTransform(base_footprint_frame_,"imu", filter_time)){ 222 | ROS_ERROR("filter time older than imu message buffer"); 223 | return false; 224 | } 225 | transformer_.lookupTransform("imu", base_footprint_frame_, filter_time, imu_meas_); 226 | if (imu_initialized_){ 227 | // convert absolute imu yaw measurement to relative imu yaw measurement 228 | Transform imu_rel_frame = filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_; 229 | ColumnVector imu_rel(3); double tmp; 230 | decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3)); 231 | decomposeTransform(imu_meas_, tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp); 232 | angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6)); 233 | diagnostics_imu_rot_rel_ = imu_rel(3); 234 | // update filter 235 | imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2)); 236 | filter_->Update(imu_meas_model_, imu_rel); 237 | } 238 | else{ 239 | imu_initialized_ = true; 240 | diagnostics_imu_rot_rel_ = 0; 241 | } 242 | imu_meas_old_ = imu_meas_; 243 | } 244 | // sensor not active 245 | else imu_initialized_ = false; 246 | 247 | 248 | 249 | // process vo measurement 250 | // ---------------------- 251 | if (vo_active){ 252 | if (!transformer_.canTransform(base_footprint_frame_,"vo", filter_time)){ 253 | ROS_ERROR("filter time older than vo message buffer"); 254 | return false; 255 | } 256 | transformer_.lookupTransform("vo", base_footprint_frame_, filter_time, vo_meas_); 257 | if (vo_initialized_){ 258 | // convert absolute vo measurements to relative vo measurements 259 | Transform vo_rel_frame = filter_estimate_old_ * vo_meas_old_.inverse() * vo_meas_; 260 | ColumnVector vo_rel(6); 261 | decomposeTransform(vo_rel_frame, vo_rel(1), vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6)); 262 | angleOverflowCorrect(vo_rel(6), filter_estimate_old_vec_(6)); 263 | // update filter 264 | vo_meas_pdf_->AdditiveNoiseSigmaSet(vo_covariance_ * pow(dt,2)); 265 | filter_->Update(vo_meas_model_, vo_rel); 266 | } 267 | else vo_initialized_ = true; 268 | vo_meas_old_ = vo_meas_; 269 | } 270 | // sensor not active 271 | else vo_initialized_ = false; 272 | 273 | 274 | 275 | // process gps measurement 276 | // ---------------------- 277 | if (gps_active){ 278 | if (!transformer_.canTransform(base_footprint_frame_,"gps", filter_time)){ 279 | ROS_ERROR("filter time older than gps message buffer"); 280 | return false; 281 | } 282 | transformer_.lookupTransform("gps", base_footprint_frame_, filter_time, gps_meas_); 283 | if (gps_initialized_){ 284 | gps_meas_pdf_->AdditiveNoiseSigmaSet(gps_covariance_ * pow(dt,2)); 285 | ColumnVector gps_vec(3); 286 | double tmp; 287 | //Take gps as an absolute measurement, do not convert to relative measurement 288 | decomposeTransform(gps_meas_, gps_vec(1), gps_vec(2), gps_vec(3), tmp, tmp, tmp); 289 | filter_->Update(gps_meas_model_, gps_vec); 290 | } 291 | else { 292 | gps_initialized_ = true; 293 | gps_meas_old_ = gps_meas_; 294 | } 295 | } 296 | // sensor not active 297 | else gps_initialized_ = false; 298 | 299 | 300 | 301 | // remember last estimate 302 | filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet(); 303 | tf::Quaternion q; 304 | q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5), filter_estimate_old_vec_(6)); 305 | filter_estimate_old_ = Transform(q, 306 | Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3))); 307 | filter_time_old_ = filter_time; 308 | addMeasurement(StampedTransform(filter_estimate_old_, filter_time, output_frame_, base_footprint_frame_)); 309 | 310 | // diagnostics 311 | diagnostics_res = true; 312 | if (odom_active && imu_active){ 313 | double diagnostics = fabs(diagnostics_odom_rot_rel_ - diagnostics_imu_rot_rel_)/dt; 314 | if (diagnostics > 0.3 && dt > 0.01){ 315 | diagnostics_res = false; 316 | } 317 | } 318 | 319 | return true; 320 | }; 321 | 322 | void OdomEstimation::addMeasurement(const StampedTransform& meas) 323 | { 324 | ROS_DEBUG("AddMeasurement from %s to %s: (%f, %f, %f) (%f, %f, %f, %f)", 325 | meas.frame_id_.c_str(), meas.child_frame_id_.c_str(), 326 | meas.getOrigin().x(), meas.getOrigin().y(), meas.getOrigin().z(), 327 | meas.getRotation().x(), meas.getRotation().y(), 328 | meas.getRotation().z(), meas.getRotation().w()); 329 | transformer_.setTransform( meas ); 330 | } 331 | 332 | void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar) 333 | { 334 | // check covariance 335 | for (unsigned int i=0; iPostGet()->CovarianceGet(); 397 | for (unsigned int i=0; i<6; i++) 398 | for (unsigned int j=0; j<6; j++) 399 | estimate.pose.covariance[6*i+j] = covar(i+1,j+1); 400 | }; 401 | 402 | // correct for angle overflow 403 | void OdomEstimation::angleOverflowCorrect(double& a, double ref) 404 | { 405 | while ((a-ref) > M_PI) a -= 2*M_PI; 406 | while ((a-ref) < -M_PI) a += 2*M_PI; 407 | }; 408 | 409 | // decompose Transform into x,y,z,Rx,Ry,Rz 410 | void OdomEstimation::decomposeTransform(const StampedTransform& trans, 411 | double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){ 412 | x = trans.getOrigin().x(); 413 | y = trans.getOrigin().y(); 414 | z = trans.getOrigin().z(); 415 | trans.getBasis().getEulerYPR(Rz, Ry, Rx); 416 | }; 417 | 418 | // decompose Transform into x,y,z,Rx,Ry,Rz 419 | void OdomEstimation::decomposeTransform(const Transform& trans, 420 | double& x, double& y, double&z, double&Rx, double& Ry, double& Rz){ 421 | x = trans.getOrigin().x(); 422 | y = trans.getOrigin().y(); 423 | z = trans.getOrigin().z(); 424 | trans.getBasis().getEulerYPR(Rz, Ry, Rx); 425 | }; 426 | 427 | void OdomEstimation::setOutputFrame(const std::string& output_frame){ 428 | output_frame_ = output_frame; 429 | }; 430 | 431 | void OdomEstimation::setBaseFootprintFrame(const std::string& base_frame){ 432 | base_footprint_frame_ = base_frame; 433 | }; 434 | 435 | }; // namespace 436 | -------------------------------------------------------------------------------- /src/odom_estimation_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | 39 | 40 | using namespace MatrixWrapper; 41 | using namespace std; 42 | using namespace ros; 43 | using namespace tf; 44 | 45 | 46 | static const double EPS = 1e-5; 47 | 48 | 49 | //#define __EKF_DEBUG_FILE__ 50 | 51 | namespace estimation 52 | { 53 | // constructor 54 | OdomEstimationNode::OdomEstimationNode() 55 | : odom_active_(false), 56 | imu_active_(false), 57 | vo_active_(false), 58 | gps_active_(false), 59 | odom_initializing_(false), 60 | imu_initializing_(false), 61 | vo_initializing_(false), 62 | gps_initializing_(false), 63 | odom_covariance_(6), 64 | imu_covariance_(3), 65 | vo_covariance_(6), 66 | gps_covariance_(3), 67 | odom_callback_counter_(0), 68 | imu_callback_counter_(0), 69 | vo_callback_counter_(0), 70 | gps_callback_counter_(0), 71 | ekf_sent_counter_(0) 72 | { 73 | ros::NodeHandle nh_private("~"); 74 | ros::NodeHandle nh; 75 | 76 | // paramters 77 | nh_private.param("output_frame", output_frame_, std::string("odom_combined")); 78 | nh_private.param("base_footprint_frame", base_footprint_frame_, std::string("base_footprint")); 79 | nh_private.param("sensor_timeout", timeout_, 1.0); 80 | nh_private.param("odom_used", odom_used_, true); 81 | nh_private.param("imu_used", imu_used_, true); 82 | nh_private.param("vo_used", vo_used_, true); 83 | nh_private.param("gps_used", gps_used_, false); 84 | nh_private.param("debug", debug_, false); 85 | nh_private.param("self_diagnose", self_diagnose_, false); 86 | double freq; 87 | nh_private.param("freq", freq, 30.0); 88 | 89 | tf_prefix_ = tf::getPrefixParam(nh_private); 90 | output_frame_ = tf::resolve(tf_prefix_, output_frame_); 91 | base_footprint_frame_ = tf::resolve(tf_prefix_, base_footprint_frame_); 92 | 93 | ROS_INFO_STREAM("output frame: " << output_frame_); 94 | ROS_INFO_STREAM("base frame: " << base_footprint_frame_); 95 | 96 | // set output frame and base frame names in OdomEstimation filter 97 | // so that user-defined tf frames are respected 98 | my_filter_.setOutputFrame(output_frame_); 99 | my_filter_.setBaseFootprintFrame(base_footprint_frame_); 100 | 101 | timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this); 102 | 103 | // advertise our estimation 104 | pose_pub_ = nh_private.advertise("odom_combined", 10); 105 | 106 | // initialize 107 | filter_stamp_ = Time::now(); 108 | 109 | // subscribe to odom messages 110 | if (odom_used_){ 111 | ROS_DEBUG("Odom sensor can be used"); 112 | odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this); 113 | } 114 | else ROS_DEBUG("Odom sensor will NOT be used"); 115 | 116 | // subscribe to imu messages 117 | if (imu_used_){ 118 | ROS_DEBUG("Imu sensor can be used"); 119 | imu_sub_ = nh.subscribe("imu_data", 10, &OdomEstimationNode::imuCallback, this); 120 | } 121 | else ROS_DEBUG("Imu sensor will NOT be used"); 122 | 123 | // subscribe to vo messages 124 | if (vo_used_){ 125 | ROS_DEBUG("VO sensor can be used"); 126 | vo_sub_ = nh.subscribe("vo", 10, &OdomEstimationNode::voCallback, this); 127 | } 128 | else ROS_DEBUG("VO sensor will NOT be used"); 129 | 130 | if (gps_used_){ 131 | ROS_DEBUG("GPS sensor can be used"); 132 | gps_sub_ = nh.subscribe("gps", 10, &OdomEstimationNode::gpsCallback, this); 133 | } 134 | else ROS_DEBUG("GPS sensor will NOT be used"); 135 | 136 | 137 | // publish state service 138 | state_srv_ = nh_private.advertiseService("get_status", &OdomEstimationNode::getStatus, this); 139 | 140 | if (debug_){ 141 | // open files for debugging 142 | odom_file_.open("/tmp/odom_file.txt"); 143 | imu_file_.open("/tmp/imu_file.txt"); 144 | vo_file_.open("/tmp/vo_file.txt"); 145 | gps_file_.open("/tmp/gps_file.txt"); 146 | corr_file_.open("/tmp/corr_file.txt"); 147 | 148 | 149 | } 150 | }; 151 | 152 | 153 | 154 | 155 | // destructor 156 | OdomEstimationNode::~OdomEstimationNode(){ 157 | 158 | if (debug_){ 159 | // close files for debugging 160 | odom_file_.close(); 161 | imu_file_.close(); 162 | gps_file_.close(); 163 | vo_file_.close(); 164 | corr_file_.close(); 165 | } 166 | }; 167 | 168 | 169 | 170 | 171 | 172 | // callback function for odom data 173 | void OdomEstimationNode::odomCallback(const OdomConstPtr& odom) 174 | { 175 | odom_callback_counter_++; 176 | 177 | ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec()); 178 | assert(odom_used_); 179 | 180 | // receive data 181 | odom_stamp_ = odom->header.stamp; 182 | odom_time_ = Time::now(); 183 | Quaternion q; 184 | tf::quaternionMsgToTF(odom->pose.pose.orientation, q); 185 | odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0)); 186 | for (unsigned int i=0; i<6; i++) 187 | for (unsigned int j=0; j<6; j++) 188 | odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j]; 189 | 190 | my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_); 191 | 192 | // activate odom 193 | if (!odom_active_) { 194 | if (!odom_initializing_){ 195 | odom_initializing_ = true; 196 | odom_init_stamp_ = odom_stamp_; 197 | ROS_INFO("Initializing Odom sensor"); 198 | } 199 | if ( filter_stamp_ >= odom_init_stamp_){ 200 | odom_active_ = true; 201 | odom_initializing_ = false; 202 | ROS_INFO("Odom sensor activated"); 203 | } 204 | else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.", 205 | (odom_init_stamp_ - filter_stamp_).toSec()); 206 | } 207 | 208 | if (debug_){ 209 | // write to file 210 | double tmp, yaw; 211 | odom_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); 212 | odom_file_<< fixed <header.stamp; 228 | tf::Quaternion orientation; 229 | quaternionMsgToTF(imu->orientation, orientation); 230 | imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0)); 231 | for (unsigned int i=0; i<3; i++) 232 | for (unsigned int j=0; j<3; j++) 233 | imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j]; 234 | 235 | // Transforms imu data to base_footprint frame 236 | if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){ 237 | // warn when imu was already activated, not when imu is not active yet 238 | if (imu_active_) 239 | ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str()); 240 | else if (my_filter_.isInitialized()) 241 | ROS_WARN("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str()); 242 | else 243 | ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str()); 244 | return; 245 | } 246 | StampedTransform base_imu_offset; 247 | robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset); 248 | imu_meas_ = imu_meas_ * base_imu_offset; 249 | 250 | imu_time_ = Time::now(); 251 | 252 | // manually set covariance untile imu sends covariance 253 | if (imu_covariance_(1,1) == 0.0){ 254 | SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0; 255 | measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec 256 | measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec 257 | measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec 258 | imu_covariance_ = measNoiseImu_Cov; 259 | } 260 | 261 | my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imu"), imu_covariance_); 262 | 263 | // activate imu 264 | if (!imu_active_) { 265 | if (!imu_initializing_){ 266 | imu_initializing_ = true; 267 | imu_init_stamp_ = imu_stamp_; 268 | ROS_INFO("Initializing Imu sensor"); 269 | } 270 | if ( filter_stamp_ >= imu_init_stamp_){ 271 | imu_active_ = true; 272 | imu_initializing_ = false; 273 | ROS_INFO("Imu sensor activated"); 274 | } 275 | else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.", 276 | (imu_init_stamp_ - filter_stamp_).toSec()); 277 | } 278 | 279 | if (debug_){ 280 | // write to file 281 | double tmp, yaw; 282 | imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); 283 | imu_file_ <header.stamp; 299 | vo_time_ = Time::now(); 300 | poseMsgToTF(vo->pose.pose, vo_meas_); 301 | for (unsigned int i=0; i<6; i++) 302 | for (unsigned int j=0; j<6; j++) 303 | vo_covariance_(i+1, j+1) = vo->pose.covariance[6*i+j]; 304 | my_filter_.addMeasurement(StampedTransform(vo_meas_.inverse(), vo_stamp_, base_footprint_frame_, "vo"), vo_covariance_); 305 | 306 | // activate vo 307 | if (!vo_active_) { 308 | if (!vo_initializing_){ 309 | vo_initializing_ = true; 310 | vo_init_stamp_ = vo_stamp_; 311 | ROS_INFO("Initializing Vo sensor"); 312 | } 313 | if (filter_stamp_ >= vo_init_stamp_){ 314 | vo_active_ = true; 315 | vo_initializing_ = false; 316 | ROS_INFO("Vo sensor activated"); 317 | } 318 | else ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.", 319 | (vo_init_stamp_ - filter_stamp_).toSec()); 320 | } 321 | 322 | if (debug_){ 323 | // write to file 324 | double Rx, Ry, Rz; 325 | vo_meas_.getBasis().getEulerYPR(Rz, Ry, Rx); 326 | vo_file_ <header.stamp; 340 | gps_time_ = Time::now(); 341 | poseMsgToTF(gps->pose.pose, gps_meas_); 342 | for (unsigned int i=0; i<3; i++) 343 | for (unsigned int j=0; j<3; j++) 344 | gps_covariance_(i+1, j+1) = gps->pose.covariance[6*i+j]; 345 | my_filter_.addMeasurement(StampedTransform(gps_meas_.inverse(), gps_stamp_, base_footprint_frame_, "gps"), gps_covariance_); 346 | 347 | // activate gps 348 | if (!gps_active_) { 349 | if (!gps_initializing_){ 350 | gps_initializing_ = true; 351 | gps_init_stamp_ = gps_stamp_; 352 | ROS_INFO("Initializing GPS sensor"); 353 | } 354 | if (filter_stamp_ >= gps_init_stamp_){ 355 | gps_active_ = true; 356 | gps_initializing_ = false; 357 | ROS_INFO("GPS sensor activated"); 358 | } 359 | else ROS_DEBUG("Waiting to activate GPS, because GPS measurements are still %f sec in the future.", 360 | (gps_init_stamp_ - filter_stamp_).toSec()); 361 | } 362 | 363 | if (debug_){ 364 | // write to file 365 | gps_file_ < 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff); 380 | } 381 | 382 | // initial value for filter stamp; keep this stamp when no sensors are active 383 | filter_stamp_ = Time::now(); 384 | 385 | // check which sensors are still active 386 | if ((odom_active_ || odom_initializing_) && 387 | (Time::now() - odom_time_).toSec() > timeout_){ 388 | odom_active_ = false; odom_initializing_ = false; 389 | ROS_INFO("Odom sensor not active any more"); 390 | } 391 | if ((imu_active_ || imu_initializing_) && 392 | (Time::now() - imu_time_).toSec() > timeout_){ 393 | imu_active_ = false; imu_initializing_ = false; 394 | ROS_INFO("Imu sensor not active any more"); 395 | } 396 | if ((vo_active_ || vo_initializing_) && 397 | (Time::now() - vo_time_).toSec() > timeout_){ 398 | vo_active_ = false; vo_initializing_ = false; 399 | ROS_INFO("VO sensor not active any more"); 400 | } 401 | 402 | if ((gps_active_ || gps_initializing_) && 403 | (Time::now() - gps_time_).toSec() > timeout_){ 404 | gps_active_ = false; gps_initializing_ = false; 405 | ROS_INFO("GPS sensor not active any more"); 406 | } 407 | 408 | 409 | // only update filter when one of the sensors is active 410 | if (odom_active_ || imu_active_ || vo_active_ || gps_active_){ 411 | 412 | // update filter at time where all sensor measurements are available 413 | if (odom_active_) filter_stamp_ = min(filter_stamp_, odom_stamp_); 414 | if (imu_active_) filter_stamp_ = min(filter_stamp_, imu_stamp_); 415 | if (vo_active_) filter_stamp_ = min(filter_stamp_, vo_stamp_); 416 | if (gps_active_) filter_stamp_ = min(filter_stamp_, gps_stamp_); 417 | 418 | 419 | // update filter 420 | if ( my_filter_.isInitialized() ) { 421 | bool diagnostics = true; 422 | if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_, filter_stamp_, diagnostics)){ 423 | 424 | // output most recent estimate and relative covariance 425 | my_filter_.getEstimate(output_); 426 | pose_pub_.publish(output_); 427 | ekf_sent_counter_++; 428 | 429 | // broadcast most recent estimate to TransformArray 430 | StampedTransform tmp; 431 | my_filter_.getEstimate(ros::Time(), tmp); 432 | if(!vo_active_ && !gps_active_) 433 | tmp.getOrigin().setZ(0.0); 434 | odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, base_footprint_frame_)); 435 | 436 | if (debug_){ 437 | // write to file 438 | ColumnVector estimate; 439 | my_filter_.getEstimate(estimate); 440 | corr_file_ << fixed << setprecision(5)< 38 | #include 39 | #include "ros/ros.h" 40 | #include "nav_msgs/Odometry.h" 41 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 42 | 43 | #include 44 | 45 | using namespace ros; 46 | 47 | 48 | static const double time_end = 1264198883.0; 49 | static const double ekf_duration = 62.0; 50 | static const double EPS_trans_x = 0.02; 51 | static const double EPS_trans_y = 0.04; 52 | static const double EPS_trans_z = 0.00001; 53 | static const double EPS_rot_x = 0.005; 54 | static const double EPS_rot_y = 0.005; 55 | static const double EPS_rot_z = 0.005; 56 | static const double EPS_rot_w = 0.005; 57 | 58 | 59 | int g_argc; 60 | char** g_argv; 61 | 62 | typedef boost::shared_ptr OdomConstPtr; 63 | typedef boost::shared_ptr EkfConstPtr; 64 | 65 | class TestEKF : public testing::Test 66 | { 67 | public: 68 | NodeHandle node_; 69 | ros::Subscriber odom_sub_, ekf_sub_; 70 | EkfConstPtr ekf_begin_, ekf_end_; 71 | OdomConstPtr odom_end_; 72 | double ekf_counter_, odom_counter_; 73 | Time ekf_time_begin_, odom_time_begin_; 74 | 75 | 76 | void OdomCallback(const OdomConstPtr& odom) 77 | { 78 | // get initial time 79 | if (odom_counter_ == 0) 80 | odom_time_begin_ = odom->header.stamp; 81 | 82 | odom_end_ = odom; 83 | 84 | // count number of callbacks 85 | odom_counter_++; 86 | } 87 | 88 | 89 | void EKFCallback(const EkfConstPtr& ekf) 90 | { 91 | // get initial time 92 | if (ekf_counter_ == 0){ 93 | ekf_time_begin_ = ekf->header.stamp; 94 | ekf_begin_ = ekf; 95 | } 96 | if (ekf->header.stamp.toSec() < time_end) 97 | ekf_end_ = ekf; 98 | 99 | // count number of callbacks 100 | ekf_counter_++; 101 | } 102 | 103 | 104 | protected: 105 | /// constructor 106 | TestEKF() 107 | { 108 | ekf_counter_ = 0; 109 | odom_counter_ = 0; 110 | 111 | } 112 | 113 | 114 | /// Destructor 115 | ~TestEKF() 116 | { 117 | } 118 | 119 | void SetUp() 120 | { 121 | ROS_INFO("Subscribing to robot_pose_ekf/odom_combined"); 122 | ekf_sub_ = node_.subscribe("/robot_pose_ekf/odom_combined", 10, &TestEKF::EKFCallback, (TestEKF*)this); 123 | 124 | ROS_INFO("Subscribing to base_odometry/odom"); 125 | odom_sub_ = node_.subscribe("base_odometry/odom", 10 , &TestEKF::OdomCallback, (TestEKF*)this); 126 | } 127 | 128 | void TearDown() 129 | { 130 | odom_sub_.shutdown(); 131 | ekf_sub_.shutdown(); 132 | } 133 | }; 134 | 135 | 136 | 137 | 138 | TEST_F(TestEKF, test) 139 | { 140 | Duration d(0.01); 141 | // wait while bag is played back 142 | ROS_INFO("Waiting for bag to start playing"); 143 | while (odom_counter_ == 0) 144 | d.sleep(); 145 | ROS_INFO("Detected that bag is playing"); 146 | 147 | ROS_INFO("Waiting untile end time is reached"); 148 | while( odom_end_->header.stamp.toSec() < time_end){ 149 | d.sleep(); 150 | } 151 | ROS_INFO("End time reached"); 152 | // give filter some time to catch up 153 | WallDuration(2.0).sleep(); 154 | 155 | // check if callback was called enough times 156 | ROS_INFO("Number of ekf callbacks: %f", ekf_counter_); 157 | EXPECT_GT(ekf_counter_, 500); 158 | 159 | // check if time interval is correct 160 | ROS_INFO("Ekf duration: %f", ekf_duration); 161 | EXPECT_LT(Duration(ekf_end_->header.stamp - ekf_time_begin_).toSec(), ekf_duration * 1.25); 162 | EXPECT_GT(Duration(ekf_end_->header.stamp - ekf_time_begin_).toSec(), ekf_duration * 0.85); 163 | 164 | // check if ekf time is same as odom time 165 | EXPECT_NEAR(ekf_time_begin_.toSec(), odom_time_begin_.toSec(), 1.0); 166 | EXPECT_NEAR(ekf_end_->header.stamp.toSec(), time_end, 1.0); 167 | 168 | // check filter result 169 | ROS_INFO("%f %f %f %f %f %f %f -- %f",ekf_end_->pose.pose.position.x, ekf_end_->pose.pose.position.y, ekf_end_->pose.pose.position.z, 170 | ekf_end_->pose.pose.orientation.x, ekf_end_->pose.pose.orientation.y, ekf_end_->pose.pose.orientation.z, ekf_end_->pose.pose.orientation.w, 171 | ekf_end_->header.stamp.toSec()); 172 | EXPECT_NEAR(ekf_end_->pose.pose.position.x, -0.0586126, EPS_trans_x); 173 | EXPECT_NEAR(ekf_end_->pose.pose.position.y, 0.0124321, EPS_trans_y); 174 | EXPECT_NEAR(ekf_end_->pose.pose.position.z, 0.0, EPS_trans_z); 175 | EXPECT_NEAR(ekf_end_->pose.pose.orientation.x, 0.00419421, EPS_rot_x); 176 | EXPECT_NEAR(ekf_end_->pose.pose.orientation.y, 0.00810739, EPS_rot_y); 177 | EXPECT_NEAR(ekf_end_->pose.pose.orientation.z, -0.0440686, EPS_rot_z); 178 | EXPECT_NEAR(ekf_end_->pose.pose.orientation.w, 0.998987, EPS_rot_w); 179 | 180 | SUCCEED(); 181 | } 182 | 183 | 184 | 185 | 186 | int main(int argc, char** argv) 187 | { 188 | testing::InitGoogleTest(&argc, argv); 189 | g_argc = argc; 190 | g_argv = argv; 191 | 192 | init(g_argc, g_argv, "testEKF"); 193 | 194 | boost::thread spinner(boost::bind(&ros::spin)); 195 | 196 | int res = RUN_ALL_TESTS(); 197 | spinner.interrupt(); 198 | spinner.join(); 199 | 200 | return res; 201 | } 202 | -------------------------------------------------------------------------------- /test/test_robot_pose_ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /test/test_robot_pose_ekf_zero_covariance.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Wim Meeussen */ 36 | 37 | #include 38 | #include 39 | #include "ros/ros.h" 40 | #include "nav_msgs/Odometry.h" 41 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 42 | 43 | #include 44 | 45 | using namespace ros; 46 | 47 | 48 | int g_argc; 49 | char** g_argv; 50 | 51 | typedef boost::shared_ptr EkfConstPtr; 52 | 53 | class TestEKF : public testing::Test 54 | { 55 | public: 56 | NodeHandle node_; 57 | ros::Subscriber ekf_sub_; 58 | double ekf_counter_; 59 | 60 | 61 | void EKFCallback(const EkfConstPtr& ekf) 62 | { 63 | // count number of callbacks 64 | ekf_counter_++; 65 | } 66 | 67 | 68 | protected: 69 | /// constructor 70 | TestEKF() 71 | { 72 | ekf_counter_ = 0; 73 | 74 | } 75 | 76 | 77 | /// Destructor 78 | ~TestEKF() 79 | { 80 | } 81 | }; 82 | 83 | 84 | 85 | 86 | TEST_F(TestEKF, test) 87 | { 88 | ROS_INFO("Subscribing to robot_pose_ekf/odom_combined"); 89 | ekf_sub_ = node_.subscribe("/robot_pose_ekf/odom_combined", 10, &TestEKF::EKFCallback, (TestEKF*)this); 90 | 91 | // wait for 20 seconds 92 | ROS_INFO("Waiting for 20 seconds while bag is playing"); 93 | ros::Duration(20).sleep(); 94 | ROS_INFO("End time reached"); 95 | 96 | EXPECT_EQ(ekf_counter_, 0); 97 | 98 | SUCCEED(); 99 | } 100 | 101 | 102 | 103 | 104 | int main(int argc, char** argv) 105 | { 106 | testing::InitGoogleTest(&argc, argv); 107 | g_argc = argc; 108 | g_argv = argv; 109 | 110 | init(g_argc, g_argv, "testEKF"); 111 | 112 | boost::thread spinner(boost::bind(&ros::spin)); 113 | 114 | int res = RUN_ALL_TESTS(); 115 | spinner.interrupt(); 116 | spinner.join(); 117 | 118 | return res; 119 | } 120 | -------------------------------------------------------------------------------- /test/test_robot_pose_ekf_zero_covariance.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | --------------------------------------------------------------------------------