From 8566b0d6839d7e874b8fce4e520d0cbdf6da44df Mon Sep 17 00:00:00 2001 From: justin Date: Tue, 27 Feb 2024 16:02:44 -0500 Subject: [PATCH] change tutorial --- .../md__home_justin_code_drift_README.html | 416 +++++----- ...tutorial_inekf_imu_and_legged_kin_ros.html | 719 ++++++++++++------ ...orials_tutorial_inekf_imu_and_vel_ros.html | 687 +++++++++++------ ...unl_code_curly_state_estimator_README.html | 137 ++++ ...de_curly_state_estimator_doc_tutorial.html | 432 +++++++++++ ...tutorial_inekf_imu_and_legged_kin_ros.html | 470 ++++++++++++ ...or_doc_tutorial_inekf_imu_and_vel_ros.html | 457 +++++++++++ _site/feed.xml | 2 +- _site/sitemap.xml | 22 +- .../md__home_justin_code_drift_README.html | 416 +++++----- ...tutorial_inekf_imu_and_legged_kin_ros.html | 719 ++++++++++++------ ...orials_tutorial_inekf_imu_and_vel_ros.html | 687 +++++++++++------ ...de_curly_state_estimator_doc_tutorial.html | 656 ++++++++++------ ...tutorial_inekf_imu_and_legged_kin_ros.html | 712 +++++++++++------ ...or_doc_tutorial_inekf_imu_and_vel_ros.html | 692 +++++++++++------ 15 files changed, 5168 insertions(+), 2056 deletions(-) create mode 100644 _site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_README.html create mode 100644 _site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html create mode 100644 _site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html create mode 100644 _site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html diff --git a/_site/doxygen/html/md__home_justin_code_drift_README.html b/_site/doxygen/html/md__home_justin_code_drift_README.html index a04cab4..fdd7e3c 100644 --- a/_site/doxygen/html/md__home_justin_code_drift_README.html +++ b/_site/doxygen/html/md__home_justin_code_drift_README.html @@ -1,184 +1,250 @@ - + + - - - - -DRIFT: DRIFT: Dead Reckoning In Field Time - - - - - - - + + + + + DRIFT: DRIFT: Dead Reckoning In Field Time + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + +
+
- -
- -
+ +
+ +
-
-
-
-
DRIFT: Dead Reckoning In Field Time
-
-
-

all_robots

-

-Description

-

DRIFT is a real-time symmetry-preserving propioceptive state estimation framework. The current implementation is based on the Invariant Kalman Filtering (InEKF). By default, DRIFT supports legged robots, differential-drive wheeled robots, full-size vehicles with shaft encoders and marine robots with Doppler Velocity Log (DVL).

-

DRIFT is designed to be modular and easy to expand to different platforms. It can be used as a standalone C++ library. Alternatively, we provide a ROS1 wrapper for easy communication between sensors.

-

-Framework

-

flow_chart

-

-Run Time Analysis

-

We perform runtime evaluations using a personal laptop with an Intel i5-11400H CPU and an NVIDIA Jetson AGX Xavier (CPU). DRIFT can operate at an extremely high frequency using CPU-only computation, even on the resourced-constrained Jetson AGX Xavier. For the optional contact estimator, the inference speed on an NVIDIA RTX 3090 GPU is approximately 1100 Hz, and the inference speed on a Jetson AGX Xavier (GPU) is around 830 Hz after TensorRT optimization.

-

run_time

-

-Dependencies

-

We have tested the library in Ubuntu 20.04 and 22.04, but it should be easy to compile in other platforms.

-
-

-
-

-C++17 Compiler

-

We use the threading functionalities of C++17.

-
-

-
-

-Eigen3

-

Required by header files. Download and install instructions can be found at: http://eigen.tuxfamily.org. Requires at least 3.1.0.

-
-

-
-

-Yaml-cpp

-

Required by header files. Download and install instructions can be found at: https://github.com/jbeder/yaml-cpp.

-
-

-
-

-ROS1 (Optional)

-

Building with ROS1 is optional. Instructions are found below.

-

-Building DRIFT library

-

Clone the repository:

git clone https://github.com/UMich-CURLY/drift.git
-

Create another directory which we will name 'build' and use cmake and make to compile an build project:

-
mkdir build
-
cd build
-
cmake ..
-
make -j4
-

-Install the library

-

After building the library, you can install the library to the system. This will allow other projects to find the library without needing to specify the path to the library.

-
sudo make install
-

Then, you can include the library in your project by adding the following line to your CMakeLists.txt file:

find_package(drift REQUIRED)
-

-ROS

-

-Examples

-

We provide several examples in the ROS/examples directory.

-

-Building the ROS1 node

-
    -
  1. Add /ROS/drift to the ROS_PACKAGE_PATH environment variable. Open your ~/.bashrc file in a text editor and add the following line to the end. Replace PATH/TO with the directory path to where you cloned drift:
    2. -
-
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/TO/drift/ROS/drift
-

Then

source ~/.bashrc
-
    -
  1. Execute build_ros.sh script in the repository root directory:
    2. -
-
cd <PATH>/<TO>/drift
-
chmod +x build_ros.sh
-
./build_ros.sh
-

-Run examples

-

Clearpath Husky robot:

rosrun drift husky
-

Fetch robot with the gyro filter:

rosrun drift fetch
-

Full-size vehicle:

rosrun drift neya
-

MIT mini-cheetah robot:

rosrun drift mini_cheetah
-

Girona500 (Marine robot):

rosrun drift girona500
-

-Run the repo with your own robots:

-

Please refer to the tutorial here: https://umich-curly.github.io/DRIFT_Website/tutorials/.

-

-Contact Estimation

-

The contact estimation and the contact data set can be found in https://github.com/UMich-CURLY/deep-contact-estimator.

-

-Citations

-

If you find this work useful, please kindly cite the following papers

-
    -
  • Tzu-Yuan Lin, Tingjun Li, Wenzhe Tong, and Maani Ghaffari. "Proprioceptive Invariant Robot State Estimation." arXiv preprint arXiv:2311.04320 (2023). (Under review for Transaction on Robotics)
    @article{lin2023proprioceptive,
    -
    title={Proprioceptive Invariant Robot State Estimation},
    -
    author={Lin, Tzu-Yuan and Li, Tingjun and Tong, Wenzhe and Ghaffari, Maani},
    -
    journal={arXiv preprint arXiv:2311.04320},
    -
    year={2023}
    -
    }
    -
    • -
  • Tzu-Yuan Lin, Ray Zhang, Justin Yu, and Maani Ghaffari. "Legged Robot State Estimation using Invariant Kalman Filtering and Learned Contact Events." In Conference on robot learning. PMLR, 2021
    @inproceedings{
    -
    lin2021legged,
    -
    title={Legged Robot State Estimation using Invariant Kalman Filtering and Learned Contact Events},
    -
    author={Tzu-Yuan Lin and Ray Zhang and Justin Yu and Maani Ghaffari},
    -
    booktitle={5th Annual Conference on Robot Learning },
    -
    year={2021},
    -
    url={https://openreview.net/forum?id=yt3tDB67lc5}
    -
    }
    -
    • -
-

-License

-

DRIFT is released under a BSD 3-Clause License.

-
-
- -
-Generated by   -doxygen - 1.8.17 -
+ +
+
+
+
DRIFT: Dead Reckoning In Field Time
+
+
+
+
+

all_robots

+

+ Description

+

DRIFT is a real-time symmetry-preserving propioceptive state estimation framework. The current implementation + is based on the Invariant + Kalman Filtering (InEKF). By default, DRIFT supports legged robots, differential-drive wheeled robots, + full-size vehicles with shaft encoders and marine robots with Doppler Velocity Log (DVL).

+

DRIFT is designed to be modular and easy to expand to different platforms. It can be used as a standalone C++ + library. Alternatively, we provide a ROS1 wrapper for easy communication between sensors.

+

+ Framework

+

flow_chart

+

+ Run Time Analysis

+

We perform runtime evaluations using a personal laptop with an Intel i5-11400H CPU and an NVIDIA Jetson AGX + Xavier (CPU). DRIFT can operate at an extremely high frequency using CPU-only computation, even on the + resourced-constrained Jetson AGX Xavier. For the optional contact estimator, the inference speed on an NVIDIA + RTX 3090 GPU is approximately 1100 Hz, and the inference speed on a Jetson AGX Xavier (GPU) is around 830 Hz + after TensorRT optimization.

+

run_time

+

+ Dependencies

+

We have tested the library in Ubuntu 20.04 and 22.04, but it should be easy to compile in other + platforms.

+
+

+
+

+ C++17 Compiler

+

We use the threading functionalities of C++17.

+
+

+
+

+ Eigen3

+

Required by header files. Download and install instructions can be found at: http://eigen.tuxfamily.org. Requires at least 3.1.0.

+
+

+
+

+ Yaml-cpp

+

Required by header files. Download and install instructions can be found at: https://github.com/jbeder/yaml-cpp.

+
+

+
+

+ ROS1 (Optional)

+

Building with ROS1 is optional. Instructions are found below.

+

+ Building DRIFT library

+

Clone the repository:

+
+
git clone https://github.com/UMich-CURLY/drift.git
+
+

Create another directory which we will name 'build' and use cmake and make to compile an build project:

+
+
mkdir build
+
cd build
+
cmake ..
+
make -j4
+
+

+ Install the library

+

After building the library, you can install the library to the system. This will allow other projects to find + the library without needing to specify the path to the library.

+
+
sudo make install
+
+

Then, you can include the library in your project by adding the following line to your CMakeLists.txt file: +

+
+
find_package(drift REQUIRED)
+
+

+ ROS

+

+ Examples

+

We provide several examples in the ROS/examples directory.

+

+ Building the ROS1 node

+
    +
  1. Add /ROS/drift to the ROS_PACKAGE_PATH environment variable. Open your ~/.bashrc + file in a text editor and add the following line to the end. Replace PATH/TO with the directory path to + where you cloned drift:
    2. +
+
+
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/TO/drift/ROS/drift
+
+

Then

+
+
source ~/.bashrc
+
+
    +
  1. Execute build_ros.sh script in the repository root directory:
    2. +
+
+
cd <PATH>/<TO>/drift
+
chmod +x build_ros.sh
+
./build_ros.sh
+
+

+ Run examples

+

Clearpath Husky robot:

+
+
rosrun drift husky
+
+

Fetch robot with the gyro filter:

+
+
rosrun drift fetch
+
+

Full-size vehicle:

+
+
rosrun drift neya
+
+

MIT mini-cheetah robot:

+
+
rosrun drift mini_cheetah
+
+

Girona500 (Marine robot):

+
+
rosrun drift girona500
+
+

+ Run the repo with your own robots:

+

Please refer to the tutorial here: https://umich-curly.github.io/DRIFT_Website/tutorials/. +

+

+ Contact Estimation

+

The contact estimation and the contact data set can be found in https://github.com/UMich-CURLY/deep-contact-estimator. +

+

+ Citations

+

If you find this work useful, please kindly cite the following papers

+
    +
  • Tzu-Yuan Lin, Tingjun Li, Wenzhe Tong, and Maani Ghaffari. "Proprioceptive Invariant Robot State + Estimation." arXiv preprint arXiv:2311.04320 (2023). (Under review for Transaction on Robotics)
    +
    @article{lin2023proprioceptive,
    +
    title={Proprioceptive Invariant Robot State Estimation},
    +
    author={Lin, Tzu-Yuan and Li, Tingjun and Tong, Wenzhe and Ghaffari, Maani},
    +
    journal={arXiv preprint arXiv:2311.04320},
    +
    year={2023}
    +
    }
    +
    • +
  • Tzu-Yuan Lin, Ray Zhang, Justin Yu, and Maani Ghaffari. "Legged Robot State Estimation using Invariant + Kalman Filtering and Learned Contact Events." In Conference on robot learning. PMLR, 2021
    +
    @inproceedings{
    +
    lin2021legged,
    +
    title={Legged Robot State Estimation using Invariant Kalman Filtering and Learned + Contact Events},
    +
    author={Tzu-Yuan Lin and Ray Zhang and Justin Yu and Maani Ghaffari},
    +
    booktitle={5th Annual Conference on Robot Learning },
    +
    year={2021},
    +
    url={https://openreview.net/forum?id=yt3tDB67lc5}
    +
    }
    +
    • +
+

+ License

+

DRIFT is released under a BSD 3-Clause + License.

+
+
+
+ +
+
+ Generated by   + doxygen + 1.8.17 +
- + + \ No newline at end of file diff --git a/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html b/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html index eea6b59..3e098e3 100644 --- a/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html +++ b/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html @@ -1,259 +1,478 @@ - + + - - - - -DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) - - - - - - - + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + +
+
- -
- -
+ +
+ +
-
-
-
-
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials. In this tutorial we focus on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the drift/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the drift/config/filter/inekf/correction. The legged_kinematics_correction.yaml file contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. Let's take the mini_cheetah.cpp as an example. The full file looks like:

/* ----------------------------------------------------------------------------
-
* Copyright 2023, CURLY Lab, University of Michigan
-
* All Rights Reserved
-
* See LICENSE for the license information
-
* -------------------------------------------------------------------------- */
-
-
#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "drift/estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
std::cout << "Subscribing to imu channel..." << std::endl;
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
std::cout << "Subscribing to joint_states and contact channel..."
-
<< std::endl;
-
auto qkin_and_mutex
-
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
-
auto qkin = qkin_and_mutex.first;
-
auto qkin_mutex = qkin_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Mini Cheetah's setting:
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
inekf_estimator.add_legged_kinematics_correction(
-
qkin, qkin_mutex,
-
"config/filter/inekf/correction/"
-
"mini_cheetah_legged_kinematics_correction.yaml");
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
return 0;
-
}
-

Let's go through the code step by step.

-

-2.1 Include necessary librarys

-

The first thing we need to do is to include necessary librarys:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-

Similarly, to add a velocity subscriber, we can do:

LeggedKinQueuePair qkin_and_mutex
-
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
-
LeggedKinQueuePtr qkin = qkin_and_mutex.first;
-
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
-

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will perform the following steps:

    -
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. -
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates legged kinematics message required by the state estimator.
    3. -
-

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. They also need to create their own kinematics measurement file resembles to drift/include/kinematics/mini_cheetah_kinematics.h and drift/src/kinematics/mini_cheetah_kinematics.cpp.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Define some configurations for the state estimator
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
inekf_estimator.add_legged_kinematics_correction(
-
qkin, qkin_mutex,
-
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml");
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

REMINDER: If users have different legged robots, they need to create their own kinematics measurement file resembles to drift/include/kinematics/mini_cheetah_kinematics.h and drift/src/kinematics/mini_cheetah_kinematics.cpp. These two files as well as their corresponding robot kinematics files are used to generate kinematics information needed by the filter. Once these files are settled, the our legged kinematics correction method can be used for user defined legged robots.

-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the state estimator by:

rosrun drift <YOUR_CASE>
-

Enjoy!

-
-
-
filter::inekf::LeftInvariant
@ LeftInvariant
Definition: inekf.h:35
-
LeggedKinQueuePair
std::pair< LeggedKinQueuePtr, std::shared_ptr< std::mutex > > LeggedKinQueuePair
Definition: ros_subscriber.h:63
-
estimator
Definition: inekf_estimator.cpp:16
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: type_def.h:51
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:102
-
ros_subscriber.h
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:48
-
inekf_estimator.h
Header file for state estimator class.
-
main
int main(int argc, char **argv)
Definition: fetch.cpp:26
-
filter::inekf::ErrorType
ErrorType
Definition: inekf.h:35
-
ros_publisher.h
Header file for ROS publisher class.
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:56
-
state
Definition: robot_state.cpp:18
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: type_def.h:33
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:45
-
LeggedKinQueuePtr
std::shared_ptr< LeggedKinQueue > LeggedKinQueuePtr
Definition: type_def.h:63
- -
-Generated by   -doxygen - 1.8.17 -
+ +
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial we focus + on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + drift/config/filter/inekf/propagation. This file contains the settings for the propagation method + and users can copy the imu_propagation.yaml under that directory to start with a new set of + settings. The second is the <correction>.yaml file in the + drift/config/filter/inekf/correction. The legged_kinematics_correction.yaml file + contains the settings for the legged kinematics correction method. All the example config files contains all + the informations needed to run the state estimator with corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. + Let's take the + mini_cheetah.cpp + as an example. The full file looks like: +

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2023, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "drift/estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
int main(int + argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
std::cout << "Subscribing to imu + channel..." << std::endl;
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
std::cout << "Subscribing to joint_states and + contact channel..."
+
<< std::endl;
+
auto qkin_and_mutex
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
auto qkin = qkin_and_mutex.first;
+
auto qkin_mutex = qkin_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = + RightInvariant; +
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Mini Cheetah's setting:
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/"
+
"mini_cheetah_legged_kinematics_correction.yaml");
+
+
RobotStateQueuePtr robot_state_queue_ptr +
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
return 0;
+
}
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this + header file
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/Imu");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr + qimu = qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
LeggedKinQueuePair qkin_and_mutex +
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
LeggedKinQueuePtr qkin = + qkin_and_mutex.first;
+
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
+
+

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will + perform the following steps:

+
    +
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. +
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates + legged kinematics message required by the state estimator.
    3. +
+

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. + They also need to create their own kinematics measurement file resembles to + drift/include/kinematics/mini_cheetah_kinematics.h and + drift/src/kinematics/mini_cheetah_kinematics.cpp. +

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some configurations for the state estimator
+
inekf::ErrorType error_type = + LeftInvariant; +
+
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr robot_state_queue_ptr = + inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

REMINDER: If users have different legged robots, they need to create their own kinematics measurement + file resembles to drift/include/kinematics/mini_cheetah_kinematics.h and + drift/src/kinematics/mini_cheetah_kinematics.cpp. + These two files as well as their corresponding robot kinematics files are used to generate kinematics + information needed by the filter. Once these files are settled, the our legged kinematics correction method + can be used for user defined legged robots. +

+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean + if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the + state estimator by:

+
+
rosrun drift <YOUR_CASE>
+
+

Enjoy!

+
+
+
+
+
filter::inekf::LeftInvariant +
+
@ LeftInvariant
+
Definition: inekf.h:35
+
+
+
LeggedKinQueuePair
+
std::pair< LeggedKinQueuePtr, std::shared_ptr< std::mutex > > LeggedKinQueuePair +
+
Definition: ros_subscriber.h:63
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
IMUQueuePtr
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: type_def.h:51
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the + queue.
+
Definition: ros_subscriber.h:102
+
+
+
ros_subscriber.h
+
+
+
IMUQueuePair
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:48
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: fetch.cpp:26
+
+
+
filter::inekf::ErrorType
+
ErrorType
+
Definition: inekf.h:35
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of the + robot us...
+
Definition: inekf_estimator.h:56
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: type_def.h:33
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path.
+
Definition: ros_publisher.h:45
+
+
+
LeggedKinQueuePtr
+
std::shared_ptr< LeggedKinQueue > LeggedKinQueuePtr
+
Definition: type_def.h:63
+
+ +
+
+ Generated by   + doxygen + 1.8.17 +
- + + \ No newline at end of file diff --git a/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html b/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html index 13e4470..2218a72 100644 --- a/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html +++ b/_site/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html @@ -1,246 +1,459 @@ - + + - - - - -DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) - - - - - - - + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + +
+
- -
- -
+ +
+ +
-
-
-
-
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials. In this tutorial, we focus on using the InEKF version of the state estimator with IMU propagation and velocity correction.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the drift/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the drift/config/filter/inekf/correction. This file contains the settings for the correction method. The velocity_correction.yaml file is a good example to start with if a user is interested in using velocity message to perform correction. There is also a legged_kinematics_correction.yaml file that contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. Let's take the husky.cpp as an example. The full file looks like:

#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "drift/estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
auto qv_and_mutex
-
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
auto qv = qv_and_mutex.first;
-
auto qv_mutex = qv_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
inekf_estimator.add_velocity_correction(qv, qv_mutex,
-
"config/filter/inekf/correction/"
-
"husky_velocity_correction.yaml");
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0;
-
}
-

Let's go through the code step by step.

-

-2.1 Include necessary libraries

-

The first thing we need to do is to include necessary libraries:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-

Similarly, to add a velocity subscriber, we can do:

VelocityQueuePair qv_and_mutex = ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
VelocityQueuePtr qv = qv_and_mutex.first;
-
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
-

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the DifferentialDriveVelocityCallback function in the ros_subscriber.cpp file. This function will convert joint_states messages into body velocity according to robot's kinematic model. In our case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert joint_states messages into body velocity is:

// Husky robot's kinematic model
-
double vr = (right_front_wheel_vel + right_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vx = (vr + vl) / 2.0; // Body velocity in x direction
-

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback function and create a new function to fit their robots' kinematic model.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Define some config settings for the state estimator
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml");
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the state estimator by:

rosrun drift <YOUR_CASE>
-

Enjoy!

-
-
-
filter::inekf::LeftInvariant
@ LeftInvariant
Definition: inekf.h:35
-
VelocityQueuePair
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > VelocityQueuePair
Definition: ros_subscriber.h:52
-
estimator
Definition: inekf_estimator.cpp:16
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: type_def.h:51
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:102
-
ros_subscriber.h
-
VelocityQueuePtr
std::shared_ptr< VelocityQueue > VelocityQueuePtr
Definition: type_def.h:73
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:48
-
inekf_estimator.h
Header file for state estimator class.
-
main
int main(int argc, char **argv)
Definition: fetch.cpp:26
-
filter::inekf::ErrorType
ErrorType
Definition: inekf.h:35
-
ros_publisher.h
Header file for ROS publisher class.
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:56
-
state
Definition: robot_state.cpp:18
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: type_def.h:33
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:45
- -
-Generated by   -doxygen - 1.8.17 -
+ +
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial, we focus + on using the InEKF version of the state estimator with IMU propagation and velocity correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + drift/config/filter/inekf/propagation. This file contains the settings for the propagation method + and users can copy the imu_propagation.yaml under that directory to start with a new set of + settings. The second is the <correction>.yaml file in the + drift/config/filter/inekf/correction. This file contains the settings for the correction method. + The velocity_correction.yaml file is a good example to start with if a user is interested in + using velocity message to perform correction. There is also a legged_kinematics_correction.yaml + file that contains the settings for the legged kinematics correction method. All the example config files + contains all the informations needed to run the state estimator with corresponding propagation or correction + method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. + Let's take the + husky.cpp + as an example. The full file looks like: +

+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "drift/estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
+
int main(int + argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
auto qv_and_mutex
+
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
auto qv = qv_and_mutex.first;
+
auto qv_mutex = qv_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = + RightInvariant; +
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex,
+
"config/filter/inekf/correction/"
+
"husky_velocity_correction.yaml");
+
+
RobotStateQueuePtr robot_state_queue_ptr +
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0;
+
}
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary libraries

+

The first thing we need to do is to include necessary libraries:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this + header file
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr + qimu = qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
VelocityQueuePair qv_and_mutex = + ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
VelocityQueuePtr qv = qv_and_mutex.first; +
+
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
+
+

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the + DifferentialDriveVelocityCallback function in the + ros_subscriber.cpp file. This function will + convert joint_states messages into body velocity according to robot's kinematic model. In our + case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert + joint_states messages into body velocity is: +

+
+
// Husky robot's kinematic model
+
double vr = (right_front_wheel_vel + right_rear_wheel_vel) + / 2.0 * wheel_radius;
+
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / + 2.0 * wheel_radius;
+
double vx = (vr + vl) / 2.0; // Body + velocity in x direction
+
+

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback + function and create a new function to fit their robots' kinematic model.

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some config settings for the state estimator
+
inekf::ErrorType error_type = + LeftInvariant; +
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr robot_state_queue_ptr = + inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean + if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the + state estimator by:

+
+
rosrun drift <YOUR_CASE>
+
+

Enjoy!

+
+
+
+
+
filter::inekf::LeftInvariant +
+
@ LeftInvariant
+
Definition: inekf.h:35
+
+
+
VelocityQueuePair
+
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > VelocityQueuePair
+
Definition: ros_subscriber.h:52
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
IMUQueuePtr
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: type_def.h:51
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the + queue.
+
Definition: ros_subscriber.h:102
+
+
+
ros_subscriber.h
+
+
+
VelocityQueuePtr
+
std::shared_ptr< VelocityQueue > VelocityQueuePtr
+
Definition: type_def.h:73
+
+
+
IMUQueuePair
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:48
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: fetch.cpp:26
+
+
+
filter::inekf::ErrorType
+
ErrorType
+
Definition: inekf.h:35
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of the + robot us...
+
Definition: inekf_estimator.h:56
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: type_def.h:33
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path.
+
Definition: ros_publisher.h:45
+
+ +
+
+ Generated by   + doxygen + 1.8.17 +
- + + \ No newline at end of file diff --git a/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_README.html b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_README.html new file mode 100644 index 0000000..8613e9c --- /dev/null +++ b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_README.html @@ -0,0 +1,137 @@ + + + + + + + +DRIFT: CURLY State Estimator + + + + + + + + + +
+
+ + + + + + +
+
DRIFT +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ +
+
+
CURLY State Estimator
+
+
+

Authors: Tzu-Yuan Lin, Tingjun Li, Jonathan Tong, and Justin Yu

+

+1. License

+

CURLY State Estimator is released under a GPLv3 license.

+

+2. Dependencies

+

We have tested the library in Ubuntu 20.04 and 22.04, but it should be easy to compile in other platforms.

+
+

‍### C++17 Compiler

+
+

We use the threading functionalities of C++17.

+
+

‍### Eigen3

+
+

Required by header files. Download and install instructions can be found at: http://eigen.tuxfamily.org. Requires at least 3.1.0.

+
+

‍### Yaml-cpp

+
+

Required by header files. Download and install instructions can be found at: https://github.com/jbeder/yaml-cpp.

+
+

‍### ROS1 or ROS2 (optional)

+
+

Building with ROS1 or ROS2 is optional. Instructions are found below.

+

+3. Building CURLY State Estimator library

+

Clone the repository:

git clone https://github.com/UMich-CURLY/curly_state_estimator.git
+

Create another directory which we will name 'build' and use cmake and make to compile an build project:

+
mkdir build
+
cd build
+
cmake ..
+
make -j4
+

+4. ROS

+

+Building the ROS1 robot_state_est nodes

+
    +

  1. Add /ROS/curly_state_estimator to the ROS_PACKAGE_PATH environment variable. Open your ~/.bashrc file in a text editor and add the following line to the end. Replace PATH/TO with the directory path to where you cloned curly_state_estimator:

    +
    export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/TO/curly_state_estimator/ROS/curly_state_estimator
    +

    Then

    source ~/.bashrc
    +
    2. +

  2. Execute build_ros.sh script in the repository root directory:

    +
    cd <PATH>/<TO>/curly_state_estimator
    +
    chmod +x build_ros.sh
    +
    ./build_ros.sh
    +
    3. +
+

+Run examples:

+

Clearpath Husky robot:

rosrun curly_state_estimator ros_comm_test
+

Fetch robot:

rosrun curly_state_estimator fetch_test
+

MIT mini-cheetah robot:

rosrun curly_state_estimator leg_kin_test
+

+Run the repo with your own settings:

+

Users can add configs inside config/filter/inekf/ directory. Configs in propagation/ stores settings related to propagation methods, e.g., mini_cheetah_imu_propagation.yaml includes all the settings we need to perform imu propagation in the filter. Configs in correction stores settings related to correction methods. For example, velocity correction method settings can refer to fetch_velocity_correction.yaml or velocity_correction.yaml, while legged kinematics correction method settings can refer to mini_cheetah_legged_kinematics_correction.yaml.

+

After editting the config files, one can easily follow tutorial comments in examples in ROS/examples to create new cases!

+
+
+ +
+Generated by doxygen 1.9.7 +
+ + diff --git a/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html new file mode 100644 index 0000000..0c73711 --- /dev/null +++ b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html @@ -0,0 +1,432 @@ + + + + + + + + + DRIFT: Tutorial (With ROS) + + + + + + + + + + +
+
+ + + + + + +
+
DRIFT +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ +
+
+
+
+
Tutorial (With ROS)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the + propagation method and users can copy the imu_propagation.yaml under that directory to start with + a new set of settings. The second is the <correction>.yaml file in the + curly_state_estimator/config/filter/inekf/correction. This file contains the settings for the + correction method. The velocity_correction.yaml file is a good example to start with if a user is + interested in using velocity message to perform correction. There is also a + legged_kinematics_correction.yaml file that contains the settings for the legged kinematics + correction method. All the example config files contains all the informations needed to run the state + estimator with corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the ROS/examples directory. Let's take + the ROS/examples/husky.cpp as an example. The full file looks like:

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2022, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
+
int main(int argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
auto qv_and_mutex
+
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
auto qv = qv_and_mutex.first;
+
auto qv_mutex = qv_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = RightInvariant;
+
YAML::Node config_
+
= YAML::LoadFile("config/filter/inekf/propagation/imu_propagation.yaml");
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex);
+
inekf_estimator.add_velocity_correction(qv, qv_mutex);
+
+
RobotStateQueuePtr + robot_state_queue_ptr
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0;
+
}
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of + the robot us...
+
Definition: inekf_estimator.h:49
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path. +
+
Definition: ros_publisher.h:49
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in + the queue.
+
Definition: ros_subscriber.h:104
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: example_1.cpp:7
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: inekf_estimator.h:39
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
ros_subscriber.h
+
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "estimator/inekf_estimator.h" // + InEKF estimator, all the propagation and correction methods are also included in this header file +
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr qimu = + qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+
IMUQueuePair +
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:50
+
+
+
IMUQueuePtr +
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: ros_subscriber.h:49
+
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
VelocityQueuePair qv_and_mutex = + ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
VelocityQueuePtr qv = + qv_and_mutex.first;
+
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
+
+
VelocityQueuePtr +
+
std::shared_ptr< VelocityQueue > VelocityQueuePtr
+
Definition: ros_subscriber.h:55
+
+
+
VelocityQueuePair
+
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > + VelocityQueuePair
+
Definition: ros_subscriber.h:58
+
+
+

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the + DifferentialDriveVelocityCallback function in the + ros_subscriber.cpp file. This function will + convert joint_states messages into body velocity according to robot's kinematic model. In our + case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert + joint_states messages into body velocity is: +

+
+
// Husky robot's kinematic model
+
double vr = (right_front_wheel_vel + right_rear_wheel_vel) + / 2.0 * wheel_radius;
+
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / + 2.0 * wheel_radius;
+
double vx = (vr + vl) / 2.0; // Body + velocity in x direction
+
+

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback + function and create a new function to fit their robots' kinematic model.

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Create an InEKF estimator
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex);
+
inekf_estimator.add_velocity_correction(qv, qv_mutex);
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr + robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we + mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+
+
+
+ +
+
+ Generated by doxygen 1.9.7 +
+ + + \ No newline at end of file diff --git a/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html new file mode 100644 index 0000000..8171167 --- /dev/null +++ b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html @@ -0,0 +1,470 @@ + + + + + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) + + + + + + + + + + +
+
+ + + + + + +
+
DRIFT +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ +
+
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial we focus + on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the + propagation method and users can copy the imu_propagation.yaml under that directory to start with + a new set of settings. The second is the <correction>.yaml file in the + curly_state_estimator/config/filter/inekf/correction. The + legged_kinematics_correction.yaml file contains the settings for the legged kinematics correction + method. All the example config files contains all the informations needed to run the state estimator with + corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the + curly_state_estimator/ROS/curly_state_estimator/examples directory. Let's take the + mini_cheetah.cpp as an example. The full file looks like: +

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2022, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
int main(int argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
std::cout << "Subscribing to imu + channel..." << std::endl;
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
std::cout << "Subscribing to joint_states and + contact channel..."
+
<< std::endl;
+
auto qkin_and_mutex
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
auto qkin = qkin_and_mutex.first;
+
auto qkin_mutex = qkin_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = RightInvariant;
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Mini Cheetah's setting:
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/"
+
"mini_cheetah_legged_kinematics_correction.yaml");
+
+
RobotStateQueuePtr + robot_state_queue_ptr
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
return 0;
+
}
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of + the robot us...
+
Definition: inekf_estimator.h:49
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path. +
+
Definition: ros_publisher.h:49
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in + the queue.
+
Definition: ros_subscriber.h:104
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: example_1.cpp:7
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: inekf_estimator.h:39
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
ros_subscriber.h
+
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "estimator/inekf_estimator.h" // + InEKF estimator, all the propagation and correction methods are also included in this header file +
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/Imu");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr qimu = + qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+
IMUQueuePair +
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:50
+
+
+
IMUQueuePtr +
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: ros_subscriber.h:49
+
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
LegKinQueuePair qkin_and_mutex
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
LegKinQueuePtr qkin = + qkin_and_mutex.first;
+
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
+
+
LegKinQueuePair +
+
std::pair< LegKinQueuePtr, std::shared_ptr< std::mutex > > LegKinQueuePair +
+
Definition: ros_subscriber.h:66
+
+
+
LegKinQueuePtr +
+
std::shared_ptr< LegKinQueue > LegKinQueuePtr
+
Definition: ros_subscriber.h:64
+
+
+

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will + perform the following steps:

+
    +
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. +
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates + legged kinematics message required by the state estimator.
    3. +
+

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. + They also need to create their own kinematics measurement file resembles to + curly_state_estimator/include/kinematics/mini_cheetah_kinematics.h + and + curly_state_estimator/src/kinematics/mini_cheetah_kinematics.cpp. +

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some configurations for the state estimator
+
inekf::ErrorType error_type = LeftInvariant;
+
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr + robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

REMINDER: If users have different legged robots, they need to create their own kinematics measurement + file resembles to + curly_state_estimator/include/kinematics/mini_cheetah_kinematics.h + and + curly_state_estimator/src/kinematics/mini_cheetah_kinematics.cpp. + These two files as well as their corresponding robot kinematics files are used to generate kinematics + information needed by the filter. Once these files are settled, the our legged kinematics correction method + can be used for user defined legged robots. +

+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we + mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+
+
+
+ +
+
+ Generated by doxygen 1.9.7 +
+ + + \ No newline at end of file diff --git a/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html new file mode 100644 index 0000000..763c136 --- /dev/null +++ b/_site/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html @@ -0,0 +1,457 @@ + + + + + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) + + + + + + + + + + +
+
+ + + + + + +
+
DRIFT +
+
+
+ + + + + + + + +
+
+ + +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
+ +
+
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial, we focus + on using the InEKF version of the state estimator with IMU propagation and velocity correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the + propagation method and users can copy the imu_propagation.yaml under that directory to start with + a new set of settings. The second is the <correction>.yaml file in the + curly_state_estimator/config/filter/inekf/correction. This file contains the settings for the + correction method. The velocity_correction.yaml file is a good example to start with if a user is + interested in using velocity message to perform correction. There is also a + legged_kinematics_correction.yaml file that contains the settings for the legged kinematics + correction method. All the example config files contains all the informations needed to run the state + estimator with corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the + curly_state_estimator/ROS/curly_state_estimator/examples directory. Let's take the + husky.cpp as an example. The full file looks like: +

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2022, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
+
int main(int argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
auto qv_and_mutex
+
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
auto qv = qv_and_mutex.first;
+
auto qv_mutex = qv_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = RightInvariant;
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex,
+
"config/filter/inekf/correction/"
+
"husky_velocity_correction.yaml");
+
+
RobotStateQueuePtr + robot_state_queue_ptr
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0;
+
}
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of + the robot us...
+
Definition: inekf_estimator.h:49
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path. +
+
Definition: ros_publisher.h:49
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in + the queue.
+
Definition: ros_subscriber.h:104
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: example_1.cpp:7
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: inekf_estimator.h:39
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
ros_subscriber.h
+
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "estimator/inekf_estimator.h" // + InEKF estimator, all the propagation and correction methods are also included in this header file +
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr qimu = + qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+
IMUQueuePair +
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:50
+
+
+
IMUQueuePtr +
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: ros_subscriber.h:49
+
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
VelocityQueuePair qv_and_mutex = + ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
VelocityQueuePtr qv = + qv_and_mutex.first;
+
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
+
+
VelocityQueuePtr +
+
std::shared_ptr< VelocityQueue > VelocityQueuePtr
+
Definition: ros_subscriber.h:55
+
+
+
VelocityQueuePair
+
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > + VelocityQueuePair
+
Definition: ros_subscriber.h:58
+
+
+

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the + DifferentialDriveVelocityCallback function in the + ros_subscriber.cpp file. This function will + convert joint_states messages into body velocity according to robot's kinematic model. In our + case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert + joint_states messages into body velocity is: +

+
+
// Husky robot's kinematic model
+
double vr = (right_front_wheel_vel + right_rear_wheel_vel) + / 2.0 * wheel_radius;
+
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / + 2.0 * wheel_radius;
+
double vx = (vr + vl) / 2.0; // Body + velocity in x direction
+
+

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback + function and create a new function to fit their robots' kinematic model.

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some config settings for the state estimator
+
inekf::ErrorType error_type = LeftInvariant;
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr + robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we + mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+
+
+
+ +
+
+ Generated by doxygen 1.9.7 +
+ + + \ No newline at end of file diff --git a/_site/feed.xml b/_site/feed.xml index 12fc52b..1cb8520 100644 --- a/_site/feed.xml +++ b/_site/feed.xml @@ -1 +1 @@ -Jekyll2024-02-19T12:31:09-08:00http://localhost:4000/DRIFT_Website/feed.xmlDRIFTpersonal descriptionUM Roboticshttps://curly.engin.umich.edu/ \ No newline at end of file +Jekyll2024-02-27T13:01:45-08:00http://localhost:4000/DRIFT_Website/feed.xmlDRIFTpersonal descriptionUM Roboticshttps://curly.engin.umich.edu/ \ No newline at end of file diff --git a/_site/sitemap.xml b/_site/sitemap.xml index 26f42f2..d70be9c 100644 --- a/_site/sitemap.xml +++ b/_site/sitemap.xml @@ -1447,7 +1447,7 @@ http://localhost:4000/DRIFT_Website/doxygen/html/md__home_justin_code_drift_README.html -2024-02-19T12:04:38-08:00 +2024-02-27T13:01:42-08:00 http://localhost:4000/DRIFT_Website/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_communication_wrapper.html @@ -1455,17 +1455,33 @@ http://localhost:4000/DRIFT_Website/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html -2024-02-19T12:04:38-08:00 +2024-02-27T13:01:26-08:00 http://localhost:4000/DRIFT_Website/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html -2024-02-19T12:04:38-08:00 +2024-02-27T12:59:28-08:00 http://localhost:4000/DRIFT_Website/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_legged_kinematics.html 2024-02-19T12:04:38-08:00 +http://localhost:4000/DRIFT_Website/doxygen/html/md__home_tingjunl_code_curly_state_estimator_README.html +2024-02-19T13:47:24-08:00 + + +http://localhost:4000/DRIFT_Website/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html +2024-02-27T13:01:04-08:00 + + +http://localhost:4000/DRIFT_Website/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html +2024-02-27T13:00:50-08:00 + + +http://localhost:4000/DRIFT_Website/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html +2024-02-27T13:00:57-08:00 + + http://localhost:4000/DRIFT_Website/doxygen/html/measurement_8cpp.html 2024-02-19T12:04:42-08:00 diff --git a/doxygen/html/md__home_justin_code_drift_README.html b/doxygen/html/md__home_justin_code_drift_README.html index a04cab4..fdd7e3c 100644 --- a/doxygen/html/md__home_justin_code_drift_README.html +++ b/doxygen/html/md__home_justin_code_drift_README.html @@ -1,184 +1,250 @@ - + + - - - - -DRIFT: DRIFT: Dead Reckoning In Field Time - - - - - - - + + + + + DRIFT: DRIFT: Dead Reckoning In Field Time + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + +
+
- -
- -
+ +
+ +
-
-
-
-
DRIFT: Dead Reckoning In Field Time
-
-
-

all_robots

-

-Description

-

DRIFT is a real-time symmetry-preserving propioceptive state estimation framework. The current implementation is based on the Invariant Kalman Filtering (InEKF). By default, DRIFT supports legged robots, differential-drive wheeled robots, full-size vehicles with shaft encoders and marine robots with Doppler Velocity Log (DVL).

-

DRIFT is designed to be modular and easy to expand to different platforms. It can be used as a standalone C++ library. Alternatively, we provide a ROS1 wrapper for easy communication between sensors.

-

-Framework

-

flow_chart

-

-Run Time Analysis

-

We perform runtime evaluations using a personal laptop with an Intel i5-11400H CPU and an NVIDIA Jetson AGX Xavier (CPU). DRIFT can operate at an extremely high frequency using CPU-only computation, even on the resourced-constrained Jetson AGX Xavier. For the optional contact estimator, the inference speed on an NVIDIA RTX 3090 GPU is approximately 1100 Hz, and the inference speed on a Jetson AGX Xavier (GPU) is around 830 Hz after TensorRT optimization.

-

run_time

-

-Dependencies

-

We have tested the library in Ubuntu 20.04 and 22.04, but it should be easy to compile in other platforms.

-
-

-
-

-C++17 Compiler

-

We use the threading functionalities of C++17.

-
-

-
-

-Eigen3

-

Required by header files. Download and install instructions can be found at: http://eigen.tuxfamily.org. Requires at least 3.1.0.

-
-

-
-

-Yaml-cpp

-

Required by header files. Download and install instructions can be found at: https://github.com/jbeder/yaml-cpp.

-
-

-
-

-ROS1 (Optional)

-

Building with ROS1 is optional. Instructions are found below.

-

-Building DRIFT library

-

Clone the repository:

git clone https://github.com/UMich-CURLY/drift.git
-

Create another directory which we will name 'build' and use cmake and make to compile an build project:

-
mkdir build
-
cd build
-
cmake ..
-
make -j4
-

-Install the library

-

After building the library, you can install the library to the system. This will allow other projects to find the library without needing to specify the path to the library.

-
sudo make install
-

Then, you can include the library in your project by adding the following line to your CMakeLists.txt file:

find_package(drift REQUIRED)
-

-ROS

-

-Examples

-

We provide several examples in the ROS/examples directory.

-

-Building the ROS1 node

-
    -
  1. Add /ROS/drift to the ROS_PACKAGE_PATH environment variable. Open your ~/.bashrc file in a text editor and add the following line to the end. Replace PATH/TO with the directory path to where you cloned drift:
    2. -
-
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/TO/drift/ROS/drift
-

Then

source ~/.bashrc
-
    -
  1. Execute build_ros.sh script in the repository root directory:
    2. -
-
cd <PATH>/<TO>/drift
-
chmod +x build_ros.sh
-
./build_ros.sh
-

-Run examples

-

Clearpath Husky robot:

rosrun drift husky
-

Fetch robot with the gyro filter:

rosrun drift fetch
-

Full-size vehicle:

rosrun drift neya
-

MIT mini-cheetah robot:

rosrun drift mini_cheetah
-

Girona500 (Marine robot):

rosrun drift girona500
-

-Run the repo with your own robots:

-

Please refer to the tutorial here: https://umich-curly.github.io/DRIFT_Website/tutorials/.

-

-Contact Estimation

-

The contact estimation and the contact data set can be found in https://github.com/UMich-CURLY/deep-contact-estimator.

-

-Citations

-

If you find this work useful, please kindly cite the following papers

-
    -
  • Tzu-Yuan Lin, Tingjun Li, Wenzhe Tong, and Maani Ghaffari. "Proprioceptive Invariant Robot State Estimation." arXiv preprint arXiv:2311.04320 (2023). (Under review for Transaction on Robotics)
    @article{lin2023proprioceptive,
    -
    title={Proprioceptive Invariant Robot State Estimation},
    -
    author={Lin, Tzu-Yuan and Li, Tingjun and Tong, Wenzhe and Ghaffari, Maani},
    -
    journal={arXiv preprint arXiv:2311.04320},
    -
    year={2023}
    -
    }
    -
    • -
  • Tzu-Yuan Lin, Ray Zhang, Justin Yu, and Maani Ghaffari. "Legged Robot State Estimation using Invariant Kalman Filtering and Learned Contact Events." In Conference on robot learning. PMLR, 2021
    @inproceedings{
    -
    lin2021legged,
    -
    title={Legged Robot State Estimation using Invariant Kalman Filtering and Learned Contact Events},
    -
    author={Tzu-Yuan Lin and Ray Zhang and Justin Yu and Maani Ghaffari},
    -
    booktitle={5th Annual Conference on Robot Learning },
    -
    year={2021},
    -
    url={https://openreview.net/forum?id=yt3tDB67lc5}
    -
    }
    -
    • -
-

-License

-

DRIFT is released under a BSD 3-Clause License.

-
-
- -
-Generated by   -doxygen - 1.8.17 -
+ +
+
+
+
DRIFT: Dead Reckoning In Field Time
+
+
+
+
+

all_robots

+

+ Description

+

DRIFT is a real-time symmetry-preserving propioceptive state estimation framework. The current implementation + is based on the Invariant + Kalman Filtering (InEKF). By default, DRIFT supports legged robots, differential-drive wheeled robots, + full-size vehicles with shaft encoders and marine robots with Doppler Velocity Log (DVL).

+

DRIFT is designed to be modular and easy to expand to different platforms. It can be used as a standalone C++ + library. Alternatively, we provide a ROS1 wrapper for easy communication between sensors.

+

+ Framework

+

flow_chart

+

+ Run Time Analysis

+

We perform runtime evaluations using a personal laptop with an Intel i5-11400H CPU and an NVIDIA Jetson AGX + Xavier (CPU). DRIFT can operate at an extremely high frequency using CPU-only computation, even on the + resourced-constrained Jetson AGX Xavier. For the optional contact estimator, the inference speed on an NVIDIA + RTX 3090 GPU is approximately 1100 Hz, and the inference speed on a Jetson AGX Xavier (GPU) is around 830 Hz + after TensorRT optimization.

+

run_time

+

+ Dependencies

+

We have tested the library in Ubuntu 20.04 and 22.04, but it should be easy to compile in other + platforms.

+
+

+
+

+ C++17 Compiler

+

We use the threading functionalities of C++17.

+
+

+
+

+ Eigen3

+

Required by header files. Download and install instructions can be found at: http://eigen.tuxfamily.org. Requires at least 3.1.0.

+
+

+
+

+ Yaml-cpp

+

Required by header files. Download and install instructions can be found at: https://github.com/jbeder/yaml-cpp.

+
+

+
+

+ ROS1 (Optional)

+

Building with ROS1 is optional. Instructions are found below.

+

+ Building DRIFT library

+

Clone the repository:

+
+
git clone https://github.com/UMich-CURLY/drift.git
+
+

Create another directory which we will name 'build' and use cmake and make to compile an build project:

+
+
mkdir build
+
cd build
+
cmake ..
+
make -j4
+
+

+ Install the library

+

After building the library, you can install the library to the system. This will allow other projects to find + the library without needing to specify the path to the library.

+
+
sudo make install
+
+

Then, you can include the library in your project by adding the following line to your CMakeLists.txt file: +

+
+
find_package(drift REQUIRED)
+
+

+ ROS

+

+ Examples

+

We provide several examples in the ROS/examples directory.

+

+ Building the ROS1 node

+
    +
  1. Add /ROS/drift to the ROS_PACKAGE_PATH environment variable. Open your ~/.bashrc + file in a text editor and add the following line to the end. Replace PATH/TO with the directory path to + where you cloned drift:
    2. +
+
+
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/TO/drift/ROS/drift
+
+

Then

+
+
source ~/.bashrc
+
+
    +
  1. Execute build_ros.sh script in the repository root directory:
    2. +
+
+
cd <PATH>/<TO>/drift
+
chmod +x build_ros.sh
+
./build_ros.sh
+
+

+ Run examples

+

Clearpath Husky robot:

+
+
rosrun drift husky
+
+

Fetch robot with the gyro filter:

+
+
rosrun drift fetch
+
+

Full-size vehicle:

+
+
rosrun drift neya
+
+

MIT mini-cheetah robot:

+
+
rosrun drift mini_cheetah
+
+

Girona500 (Marine robot):

+
+
rosrun drift girona500
+
+

+ Run the repo with your own robots:

+

Please refer to the tutorial here: https://umich-curly.github.io/DRIFT_Website/tutorials/. +

+

+ Contact Estimation

+

The contact estimation and the contact data set can be found in https://github.com/UMich-CURLY/deep-contact-estimator. +

+

+ Citations

+

If you find this work useful, please kindly cite the following papers

+
    +
  • Tzu-Yuan Lin, Tingjun Li, Wenzhe Tong, and Maani Ghaffari. "Proprioceptive Invariant Robot State + Estimation." arXiv preprint arXiv:2311.04320 (2023). (Under review for Transaction on Robotics)
    +
    @article{lin2023proprioceptive,
    +
    title={Proprioceptive Invariant Robot State Estimation},
    +
    author={Lin, Tzu-Yuan and Li, Tingjun and Tong, Wenzhe and Ghaffari, Maani},
    +
    journal={arXiv preprint arXiv:2311.04320},
    +
    year={2023}
    +
    }
    +
    • +
  • Tzu-Yuan Lin, Ray Zhang, Justin Yu, and Maani Ghaffari. "Legged Robot State Estimation using Invariant + Kalman Filtering and Learned Contact Events." In Conference on robot learning. PMLR, 2021
    +
    @inproceedings{
    +
    lin2021legged,
    +
    title={Legged Robot State Estimation using Invariant Kalman Filtering and Learned + Contact Events},
    +
    author={Tzu-Yuan Lin and Ray Zhang and Justin Yu and Maani Ghaffari},
    +
    booktitle={5th Annual Conference on Robot Learning },
    +
    year={2021},
    +
    url={https://openreview.net/forum?id=yt3tDB67lc5}
    +
    }
    +
    • +
+

+ License

+

DRIFT is released under a BSD 3-Clause + License.

+
+
+
+ +
+
+ Generated by   + doxygen + 1.8.17 +
- + + \ No newline at end of file diff --git a/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html b/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html index eea6b59..3e098e3 100644 --- a/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html +++ b/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_legged_kin_ros.html @@ -1,259 +1,478 @@ - + + - - - - -DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) - - - - - - - + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + +
+
- -
- -
+ +
+ +
-
-
-
-
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials. In this tutorial we focus on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the drift/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the drift/config/filter/inekf/correction. The legged_kinematics_correction.yaml file contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. Let's take the mini_cheetah.cpp as an example. The full file looks like:

/* ----------------------------------------------------------------------------
-
* Copyright 2023, CURLY Lab, University of Michigan
-
* All Rights Reserved
-
* See LICENSE for the license information
-
* -------------------------------------------------------------------------- */
-
-
#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "drift/estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
std::cout << "Subscribing to imu channel..." << std::endl;
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
std::cout << "Subscribing to joint_states and contact channel..."
-
<< std::endl;
-
auto qkin_and_mutex
-
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
-
auto qkin = qkin_and_mutex.first;
-
auto qkin_mutex = qkin_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Mini Cheetah's setting:
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
inekf_estimator.add_legged_kinematics_correction(
-
qkin, qkin_mutex,
-
"config/filter/inekf/correction/"
-
"mini_cheetah_legged_kinematics_correction.yaml");
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
return 0;
-
}
-

Let's go through the code step by step.

-

-2.1 Include necessary librarys

-

The first thing we need to do is to include necessary librarys:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-

Similarly, to add a velocity subscriber, we can do:

LeggedKinQueuePair qkin_and_mutex
-
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
-
LeggedKinQueuePtr qkin = qkin_and_mutex.first;
-
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
-

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will perform the following steps:

    -
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. -
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates legged kinematics message required by the state estimator.
    3. -
-

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. They also need to create their own kinematics measurement file resembles to drift/include/kinematics/mini_cheetah_kinematics.h and drift/src/kinematics/mini_cheetah_kinematics.cpp.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Define some configurations for the state estimator
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
inekf_estimator.add_legged_kinematics_correction(
-
qkin, qkin_mutex,
-
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml");
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

REMINDER: If users have different legged robots, they need to create their own kinematics measurement file resembles to drift/include/kinematics/mini_cheetah_kinematics.h and drift/src/kinematics/mini_cheetah_kinematics.cpp. These two files as well as their corresponding robot kinematics files are used to generate kinematics information needed by the filter. Once these files are settled, the our legged kinematics correction method can be used for user defined legged robots.

-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the state estimator by:

rosrun drift <YOUR_CASE>
-

Enjoy!

-
-
-
filter::inekf::LeftInvariant
@ LeftInvariant
Definition: inekf.h:35
-
LeggedKinQueuePair
std::pair< LeggedKinQueuePtr, std::shared_ptr< std::mutex > > LeggedKinQueuePair
Definition: ros_subscriber.h:63
-
estimator
Definition: inekf_estimator.cpp:16
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: type_def.h:51
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:102
-
ros_subscriber.h
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:48
-
inekf_estimator.h
Header file for state estimator class.
-
main
int main(int argc, char **argv)
Definition: fetch.cpp:26
-
filter::inekf::ErrorType
ErrorType
Definition: inekf.h:35
-
ros_publisher.h
Header file for ROS publisher class.
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:56
-
state
Definition: robot_state.cpp:18
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: type_def.h:33
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:45
-
LeggedKinQueuePtr
std::shared_ptr< LeggedKinQueue > LeggedKinQueuePtr
Definition: type_def.h:63
- -
-Generated by   -doxygen - 1.8.17 -
+ +
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial we focus + on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + drift/config/filter/inekf/propagation. This file contains the settings for the propagation method + and users can copy the imu_propagation.yaml under that directory to start with a new set of + settings. The second is the <correction>.yaml file in the + drift/config/filter/inekf/correction. The legged_kinematics_correction.yaml file + contains the settings for the legged kinematics correction method. All the example config files contains all + the informations needed to run the state estimator with corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. + Let's take the + mini_cheetah.cpp + as an example. The full file looks like: +

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2023, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "drift/estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
int main(int + argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
std::cout << "Subscribing to imu + channel..." << std::endl;
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
std::cout << "Subscribing to joint_states and + contact channel..."
+
<< std::endl;
+
auto qkin_and_mutex
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
auto qkin = qkin_and_mutex.first;
+
auto qkin_mutex = qkin_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = + RightInvariant; +
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Mini Cheetah's setting:
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/"
+
"mini_cheetah_legged_kinematics_correction.yaml");
+
+
RobotStateQueuePtr robot_state_queue_ptr +
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
return 0;
+
}
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this + header file
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/Imu");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr + qimu = qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
LeggedKinQueuePair qkin_and_mutex +
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
LeggedKinQueuePtr qkin = + qkin_and_mutex.first;
+
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
+
+

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will + perform the following steps:

+
    +
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. +
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates + legged kinematics message required by the state estimator.
    3. +
+

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. + They also need to create their own kinematics measurement file resembles to + drift/include/kinematics/mini_cheetah_kinematics.h and + drift/src/kinematics/mini_cheetah_kinematics.cpp. +

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some configurations for the state estimator
+
inekf::ErrorType error_type = + LeftInvariant; +
+
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr robot_state_queue_ptr = + inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

REMINDER: If users have different legged robots, they need to create their own kinematics measurement + file resembles to drift/include/kinematics/mini_cheetah_kinematics.h and + drift/src/kinematics/mini_cheetah_kinematics.cpp. + These two files as well as their corresponding robot kinematics files are used to generate kinematics + information needed by the filter. Once these files are settled, the our legged kinematics correction method + can be used for user defined legged robots. +

+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean + if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the + state estimator by:

+
+
rosrun drift <YOUR_CASE>
+
+

Enjoy!

+
+
+
+
+
filter::inekf::LeftInvariant +
+
@ LeftInvariant
+
Definition: inekf.h:35
+
+
+
LeggedKinQueuePair
+
std::pair< LeggedKinQueuePtr, std::shared_ptr< std::mutex > > LeggedKinQueuePair +
+
Definition: ros_subscriber.h:63
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
IMUQueuePtr
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: type_def.h:51
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the + queue.
+
Definition: ros_subscriber.h:102
+
+
+
ros_subscriber.h
+
+
+
IMUQueuePair
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:48
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: fetch.cpp:26
+
+
+
filter::inekf::ErrorType
+
ErrorType
+
Definition: inekf.h:35
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of the + robot us...
+
Definition: inekf_estimator.h:56
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: type_def.h:33
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path.
+
Definition: ros_publisher.h:45
+
+
+
LeggedKinQueuePtr
+
std::shared_ptr< LeggedKinQueue > LeggedKinQueuePtr
+
Definition: type_def.h:63
+
+ +
+
+ Generated by   + doxygen + 1.8.17 +
- + + \ No newline at end of file diff --git a/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html b/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html index 13e4470..2218a72 100644 --- a/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html +++ b/doxygen/html/md__home_justin_code_drift_doc_tutorials_tutorial_inekf_imu_and_vel_ros.html @@ -1,246 +1,459 @@ - + + - - - - -DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) - - - - - - - + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + +
+
- -
- -
+ +
+ +
-
-
-
-
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials. In this tutorial, we focus on using the InEKF version of the state estimator with IMU propagation and velocity correction.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the drift/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the drift/config/filter/inekf/correction. This file contains the settings for the correction method. The velocity_correction.yaml file is a good example to start with if a user is interested in using velocity message to perform correction. There is also a legged_kinematics_correction.yaml file that contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. Let's take the husky.cpp as an example. The full file looks like:

#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "drift/estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
auto qv_and_mutex
-
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
auto qv = qv_and_mutex.first;
-
auto qv_mutex = qv_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
inekf_estimator.add_velocity_correction(qv, qv_mutex,
-
"config/filter/inekf/correction/"
-
"husky_velocity_correction.yaml");
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0;
-
}
-

Let's go through the code step by step.

-

-2.1 Include necessary libraries

-

The first thing we need to do is to include necessary libraries:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-

Similarly, to add a velocity subscriber, we can do:

VelocityQueuePair qv_and_mutex = ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
VelocityQueuePtr qv = qv_and_mutex.first;
-
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
-

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the DifferentialDriveVelocityCallback function in the ros_subscriber.cpp file. This function will convert joint_states messages into body velocity according to robot's kinematic model. In our case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert joint_states messages into body velocity is:

// Husky robot's kinematic model
-
double vr = (right_front_wheel_vel + right_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vx = (vr + vl) / 2.0; // Body velocity in x direction
-

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback function and create a new function to fit their robots' kinematic model.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Define some config settings for the state estimator
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml");
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the state estimator by:

rosrun drift <YOUR_CASE>
-

Enjoy!

-
-
-
filter::inekf::LeftInvariant
@ LeftInvariant
Definition: inekf.h:35
-
VelocityQueuePair
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > VelocityQueuePair
Definition: ros_subscriber.h:52
-
estimator
Definition: inekf_estimator.cpp:16
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: type_def.h:51
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:102
-
ros_subscriber.h
-
VelocityQueuePtr
std::shared_ptr< VelocityQueue > VelocityQueuePtr
Definition: type_def.h:73
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:48
-
inekf_estimator.h
Header file for state estimator class.
-
main
int main(int argc, char **argv)
Definition: fetch.cpp:26
-
filter::inekf::ErrorType
ErrorType
Definition: inekf.h:35
-
ros_publisher.h
Header file for ROS publisher class.
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:56
-
state
Definition: robot_state.cpp:18
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: type_def.h:33
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:45
- -
-Generated by   -doxygen - 1.8.17 -
+ +
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial, we focus + on using the InEKF version of the state estimator with IMU propagation and velocity correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + drift/config/filter/inekf/propagation. This file contains the settings for the propagation method + and users can copy the imu_propagation.yaml under that directory to start with a new set of + settings. The second is the <correction>.yaml file in the + drift/config/filter/inekf/correction. This file contains the settings for the correction method. + The velocity_correction.yaml file is a good example to start with if a user is interested in + using velocity message to perform correction. There is also a legged_kinematics_correction.yaml + file that contains the settings for the legged kinematics correction method. All the example config files + contains all the informations needed to run the state estimator with corresponding propagation or correction + method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the drift/ROS/drift/examples directory. + Let's take the + husky.cpp + as an example. The full file looks like: +

+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "drift/estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
+
int main(int + argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
auto qv_and_mutex
+
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
auto qv = qv_and_mutex.first;
+
auto qv_mutex = qv_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = + RightInvariant; +
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex,
+
"config/filter/inekf/correction/"
+
"husky_velocity_correction.yaml");
+
+
RobotStateQueuePtr robot_state_queue_ptr +
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0;
+
}
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary libraries

+

The first thing we need to do is to include necessary libraries:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "drift/estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this + header file
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr + qimu = qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
VelocityQueuePair qv_and_mutex = + ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
VelocityQueuePtr qv = qv_and_mutex.first; +
+
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
+
+

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the + DifferentialDriveVelocityCallback function in the + ros_subscriber.cpp file. This function will + convert joint_states messages into body velocity according to robot's kinematic model. In our + case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert + joint_states messages into body velocity is: +

+
+
// Husky robot's kinematic model
+
double vr = (right_front_wheel_vel + right_rear_wheel_vel) + / 2.0 * wheel_radius;
+
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / + 2.0 * wheel_radius;
+
double vx = (vr + vl) / 2.0; // Body + velocity in x direction
+
+

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback + function and create a new function to fit their robots' kinematic model.

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some config settings for the state estimator
+
inekf::ErrorType error_type = + LeftInvariant; +
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr robot_state_queue_ptr = + inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher + ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean + if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+

After writing up your own case and adding it to drift/ROS/drift/CMakeLists.txt, you can run the + state estimator by:

+
+
rosrun drift <YOUR_CASE>
+
+

Enjoy!

+
+
+
+
+
filter::inekf::LeftInvariant +
+
@ LeftInvariant
+
Definition: inekf.h:35
+
+
+
VelocityQueuePair
+
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > VelocityQueuePair
+
Definition: ros_subscriber.h:52
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
IMUQueuePtr
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: type_def.h:51
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the + queue.
+
Definition: ros_subscriber.h:102
+
+
+
ros_subscriber.h
+
+
+
VelocityQueuePtr
+
std::shared_ptr< VelocityQueue > VelocityQueuePtr
+
Definition: type_def.h:73
+
+
+
IMUQueuePair
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:48
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: fetch.cpp:26
+
+
+
filter::inekf::ErrorType
+
ErrorType
+
Definition: inekf.h:35
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of the + robot us...
+
Definition: inekf_estimator.h:56
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: type_def.h:33
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path.
+
Definition: ros_publisher.h:45
+
+ +
+
+ Generated by   + doxygen + 1.8.17 +
- + + \ No newline at end of file diff --git a/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html b/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html index f8312d3..0c73711 100644 --- a/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html +++ b/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial.html @@ -1,242 +1,432 @@ - + + - - - - -DRIFT: Tutorial (With ROS) - - - - - - - + + + + + DRIFT: Tutorial (With ROS) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + + +
+
- -
-
-
-
-
-
Loading...
-
Searching...
-
No Matches
-
-
-
-
+ +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
-
-
-
Tutorial (With ROS)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the curly_state_estimator/config/filter/inekf/correction. This file contains the settings for the correction method. The velocity_correction.yaml file is a good example to start with if a user is interested in using velocity message to perform correction. There is also a legged_kinematics_correction.yaml file that contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the ROS/examples directory. Let's take the ROS/examples/husky.cpp as an example. The full file looks like:

/* ----------------------------------------------------------------------------
-
* Copyright 2022, CURLY Lab, University of Michigan
-
* All Rights Reserved
-
* See LICENSE for the license information
-
* -------------------------------------------------------------------------- */
-
-
#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
auto qv_and_mutex
-
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
auto qv = qv_and_mutex.first;
-
auto qv_mutex = qv_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_
-
= YAML::LoadFile("config/filter/inekf/propagation/imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
inekf_estimator.add_imu_propagation(qimu, qimu_mutex);
-
inekf_estimator.add_velocity_correction(qv, qv_mutex);
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0;
-
}
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:49
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:49
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:104
-
main
int main(int argc, char **argv)
Definition: example_1.cpp:7
-
inekf_estimator.h
Header file for state estimator class.
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: inekf_estimator.h:39
-
estimator
Definition: inekf_estimator.cpp:16
-
state
Definition: robot_state.cpp:18
-
ros_publisher.h
Header file for ROS publisher class.
-
ros_subscriber.h
-

Let's go through the code step by step.

-

-2.1 Include necessary librarys

-

The first thing we need to do is to include necessary librarys:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:50
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: ros_subscriber.h:49
-

Similarly, to add a velocity subscriber, we can do:

VelocityQueuePair qv_and_mutex = ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
VelocityQueuePtr qv = qv_and_mutex.first;
-
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
-
VelocityQueuePtr
std::shared_ptr< VelocityQueue > VelocityQueuePtr
Definition: ros_subscriber.h:55
-
VelocityQueuePair
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > VelocityQueuePair
Definition: ros_subscriber.h:58
-

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the DifferentialDriveVelocityCallback function in the ros_subscriber.cpp file. This function will convert joint_states messages into body velocity according to robot's kinematic model. In our case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert joint_states messages into body velocity is:

// Husky robot's kinematic model
-
double vr = (right_front_wheel_vel + right_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vx = (vr + vl) / 2.0; // Body velocity in x direction
-

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback function and create a new function to fit their robots' kinematic model.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(qimu, qimu_mutex);
-
inekf_estimator.add_velocity_correction(qv, qv_mutex);
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-
-
- -
-Generated by doxygen 1.9.7 -
+ +
+
+
+
Tutorial (With ROS)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the + propagation method and users can copy the imu_propagation.yaml under that directory to start with + a new set of settings. The second is the <correction>.yaml file in the + curly_state_estimator/config/filter/inekf/correction. This file contains the settings for the + correction method. The velocity_correction.yaml file is a good example to start with if a user is + interested in using velocity message to perform correction. There is also a + legged_kinematics_correction.yaml file that contains the settings for the legged kinematics + correction method. All the example config files contains all the informations needed to run the state + estimator with corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the ROS/examples directory. Let's take + the ROS/examples/husky.cpp as an example. The full file looks like:

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2022, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
+
int main(int argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
auto qv_and_mutex
+
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
auto qv = qv_and_mutex.first;
+
auto qv_mutex = qv_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = RightInvariant;
+
YAML::Node config_
+
= YAML::LoadFile("config/filter/inekf/propagation/imu_propagation.yaml");
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex);
+
inekf_estimator.add_velocity_correction(qv, qv_mutex);
+
+
RobotStateQueuePtr + robot_state_queue_ptr
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0;
+
}
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of + the robot us...
+
Definition: inekf_estimator.h:49
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path. +
+
Definition: ros_publisher.h:49
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in + the queue.
+
Definition: ros_subscriber.h:104
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: example_1.cpp:7
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: inekf_estimator.h:39
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
ros_subscriber.h
+
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "estimator/inekf_estimator.h" // + InEKF estimator, all the propagation and correction methods are also included in this header file +
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr qimu = + qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+
IMUQueuePair +
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:50
+
+
+
IMUQueuePtr +
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: ros_subscriber.h:49
+
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
VelocityQueuePair qv_and_mutex = + ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
VelocityQueuePtr qv = + qv_and_mutex.first;
+
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
+
+
VelocityQueuePtr +
+
std::shared_ptr< VelocityQueue > VelocityQueuePtr
+
Definition: ros_subscriber.h:55
+
+
+
VelocityQueuePair
+
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > + VelocityQueuePair
+
Definition: ros_subscriber.h:58
+
+
+

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the + DifferentialDriveVelocityCallback function in the + ros_subscriber.cpp file. This function will + convert joint_states messages into body velocity according to robot's kinematic model. In our + case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert + joint_states messages into body velocity is: +

+
+
// Husky robot's kinematic model
+
double vr = (right_front_wheel_vel + right_rear_wheel_vel) + / 2.0 * wheel_radius;
+
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / + 2.0 * wheel_radius;
+
double vx = (vr + vl) / 2.0; // Body + velocity in x direction
+
+

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback + function and create a new function to fit their robots' kinematic model.

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Create an InEKF estimator
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex);
+
inekf_estimator.add_velocity_correction(qv, qv_mutex);
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr + robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we + mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+
+
+
+ +
+
+ Generated by doxygen 1.9.7 +
- + + \ No newline at end of file diff --git a/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html b/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html index 2936808..8171167 100644 --- a/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html +++ b/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_legged_kin_ros.html @@ -1,260 +1,470 @@ - + + - - - - -DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) - - - - - - - + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + + +
+
- -
-
-
-
-
-
Loading...
-
Searching...
-
No Matches
-
-
-
-
+ +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
-
-
-
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials. In this tutorial we focus on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the curly_state_estimator/config/filter/inekf/correction. The legged_kinematics_correction.yaml file contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the curly_state_estimator/ROS/curly_state_estimator/examples directory. Let's take the mini_cheetah.cpp as an example. The full file looks like:

/* ----------------------------------------------------------------------------
-
* Copyright 2022, CURLY Lab, University of Michigan
-
* All Rights Reserved
-
* See LICENSE for the license information
-
* -------------------------------------------------------------------------- */
-
-
#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
std::cout << "Subscribing to imu channel..." << std::endl;
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
std::cout << "Subscribing to joint_states and contact channel..."
-
<< std::endl;
-
auto qkin_and_mutex
-
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
-
auto qkin = qkin_and_mutex.first;
-
auto qkin_mutex = qkin_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Mini Cheetah's setting:
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
inekf_estimator.add_legged_kinematics_correction(
-
qkin, qkin_mutex,
-
"config/filter/inekf/correction/"
-
"mini_cheetah_legged_kinematics_correction.yaml");
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
return 0;
-
}
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:49
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:49
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:104
-
main
int main(int argc, char **argv)
Definition: example_1.cpp:7
-
inekf_estimator.h
Header file for state estimator class.
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: inekf_estimator.h:39
-
estimator
Definition: inekf_estimator.cpp:16
-
state
Definition: robot_state.cpp:18
-
ros_publisher.h
Header file for ROS publisher class.
-
ros_subscriber.h
-

Let's go through the code step by step.

-

-2.1 Include necessary librarys

-

The first thing we need to do is to include necessary librarys:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:50
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: ros_subscriber.h:49
-

Similarly, to add a velocity subscriber, we can do:

LegKinQueuePair qkin_and_mutex
-
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
-
LegKinQueuePtr qkin = qkin_and_mutex.first;
-
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
-
LegKinQueuePair
std::pair< LegKinQueuePtr, std::shared_ptr< std::mutex > > LegKinQueuePair
Definition: ros_subscriber.h:66
-
LegKinQueuePtr
std::shared_ptr< LegKinQueue > LegKinQueuePtr
Definition: ros_subscriber.h:64
-

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will perform the following steps:

    -
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. -
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates legged kinematics message required by the state estimator.
    3. -
-

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. They also need to create their own kinematics measurement file resembles to curly_state_estimator/include/kinematics/mini_cheetah_kinematics.h and curly_state_estimator/src/kinematics/mini_cheetah_kinematics.cpp.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Define some configurations for the state estimator
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml");
-
inekf_estimator.add_legged_kinematics_correction(
-
qkin, qkin_mutex,
-
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml");
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

REMINDER: If users have different legged robots, they need to create their own kinematics measurement file resembles to curly_state_estimator/include/kinematics/mini_cheetah_kinematics.h and curly_state_estimator/src/kinematics/mini_cheetah_kinematics.cpp. These two files as well as their corresponding robot kinematics files are used to generate kinematics information needed by the filter. Once these files are settled, the our legged kinematics correction method can be used for user defined legged robots.

-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-
-
- -
-Generated by doxygen 1.9.7 -
+ +
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Legged Kinematics)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial we focus + on using the InEKF version of the state estimator with IMU propagation and legged kinematics correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the + propagation method and users can copy the imu_propagation.yaml under that directory to start with + a new set of settings. The second is the <correction>.yaml file in the + curly_state_estimator/config/filter/inekf/correction. The + legged_kinematics_correction.yaml file contains the settings for the legged kinematics correction + method. All the example config files contains all the informations needed to run the state estimator with + corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the + curly_state_estimator/ROS/curly_state_estimator/examples directory. Let's take the + mini_cheetah.cpp as an example. The full file looks like: +

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2022, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
int main(int argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
std::cout << "Subscribing to imu + channel..." << std::endl;
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/Imu");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
std::cout << "Subscribing to joint_states and + contact channel..."
+
<< std::endl;
+
auto qkin_and_mutex
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
auto qkin = qkin_and_mutex.first;
+
auto qkin_mutex = qkin_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = RightInvariant;
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Mini Cheetah's setting:
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/"
+
"mini_cheetah_legged_kinematics_correction.yaml");
+
+
RobotStateQueuePtr + robot_state_queue_ptr
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
return 0;
+
}
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of + the robot us...
+
Definition: inekf_estimator.h:49
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path. +
+
Definition: ros_publisher.h:49
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in + the queue.
+
Definition: ros_subscriber.h:104
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: example_1.cpp:7
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: inekf_estimator.h:39
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
ros_subscriber.h
+
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "estimator/inekf_estimator.h" // + InEKF estimator, all the propagation and correction methods are also included in this header file +
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/Imu");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr qimu = + qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+
IMUQueuePair +
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:50
+
+
+
IMUQueuePtr +
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: ros_subscriber.h:49
+
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
LegKinQueuePair qkin_and_mutex
+
= ros_sub.AddMiniCheetahKinematicsSubscriber("/Contacts", "/JointState");
+
LegKinQueuePtr qkin = + qkin_and_mutex.first;
+
std::shared_ptr<std::mutex> qkin_mutex = qkin_and_mutex.second;
+
+
LegKinQueuePair +
+
std::pair< LegKinQueuePtr, std::shared_ptr< std::mutex > > LegKinQueuePair +
+
Definition: ros_subscriber.h:66
+
+
+
LegKinQueuePtr +
+
std::shared_ptr< LegKinQueue > LegKinQueuePtr
+
Definition: ros_subscriber.h:64
+
+
+

The AddMiniCheetahKinematicsSubscriber(<contact_topic>, <joint_state_topic>) will + perform the following steps:

+
    +
  1. Create an ApproximateTime filter to synchronize the contact and joint state data.
    2. +
  2. Call the ROSSubscriber::MiniCHeetahKinCallBack to process the synchronized data and generates + legged kinematics message required by the state estimator.
    3. +
+

REMINDER: Users needs to create a new subscriber as well as its callback function for a new robot. + They also need to create their own kinematics measurement file resembles to + curly_state_estimator/include/kinematics/mini_cheetah_kinematics.h + and + curly_state_estimator/src/kinematics/mini_cheetah_kinematics.cpp. +

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some configurations for the state estimator
+
inekf::ErrorType error_type = LeftInvariant;
+
YAML::Node config_ = YAML::LoadFile("config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
bool enable_imu_bias_update = config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/mini_cheetah_imu_propagation.yaml"); +
+
inekf_estimator.add_legged_kinematics_correction(
+
qkin, qkin_mutex,
+
"config/filter/inekf/correction/mini_cheetah_legged_kinematics_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr + robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

REMINDER: If users have different legged robots, they need to create their own kinematics measurement + file resembles to + curly_state_estimator/include/kinematics/mini_cheetah_kinematics.h + and + curly_state_estimator/src/kinematics/mini_cheetah_kinematics.cpp. + These two files as well as their corresponding robot kinematics files are used to generate kinematics + information needed by the filter. Once these files are settled, the our legged kinematics correction method + can be used for user defined legged robots. +

+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we + mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+
+
+
+ +
+
+ Generated by doxygen 1.9.7 +
- + + \ No newline at end of file diff --git a/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html b/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html index 1f9a81c..763c136 100644 --- a/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html +++ b/doxygen/html/md__home_tingjunl_code_curly_state_estimator_doc_tutorial_inekf_imu_and_vel_ros.html @@ -1,253 +1,457 @@ - + + - - - - -DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) - - - - - - - + + + + + DRIFT: [ROS] State Estimator Tutorial (InEKF version, IMU + Velocity) + + + + + + + + -
-
- - - - - - -
-
DRIFT -
-
-
- - - - - - + + + - - -
-
+ + + +
+
- -
-
-
-
-
-
Loading...
-
Searching...
-
No Matches
-
-
-
-
+ +
+
+
+
+
+
Loading...
+
Searching...
+
No Matches
+
+
+
+
-
-
-
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
-
-
-

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, please refer to the ROS tutorials. In this tutorial, we focus on using the InEKF version of the state estimator with IMU propagation and velocity correction.

-

-Step 1: Edit Configs

-

There are at least two configuration files that need to be edited before running the state estimator. The first is the <propagation>.yaml file in the curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the propagation method and users can copy the imu_propagation.yaml under that directory to start with a new set of settings. The second is the <correction>.yaml file in the curly_state_estimator/config/filter/inekf/correction. This file contains the settings for the correction method. The velocity_correction.yaml file is a good example to start with if a user is interested in using velocity message to perform correction. There is also a legged_kinematics_correction.yaml file that contains the settings for the legged kinematics correction method. All the example config files contains all the informations needed to run the state estimator with corresponding propagation or correction method.

-

-Step 2: Create a new case with existing propagation and correction methods

-

Users can create a new case by following the comments in the curly_state_estimator/ROS/curly_state_estimator/examples directory. Let's take the husky.cpp as an example. The full file looks like:

/* ----------------------------------------------------------------------------
-
* Copyright 2022, CURLY Lab, University of Michigan
-
* All Rights Reserved
-
* See LICENSE for the license information
-
* -------------------------------------------------------------------------- */
-
-
#include <ros/ros.h>
-
#include <iostream>
-
-
#include "communication/ros_publisher.h"
-
#include "communication/ros_subscriber.h"
-
#include "estimator/inekf_estimator.h"
-
-
using namespace std;
-
using namespace state;
-
using namespace estimator;
-
-
-
int main(int argc, char** argv) {
-
ros::init(argc, argv, "robot_state_est");
-
-
std::cout << "The subscriber is on!" << std::endl;
-
-
ros::NodeHandle nh;
-
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
-
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
auto qimu = qimu_and_mutex.first;
-
auto qimu_mutex = qimu_and_mutex.second;
-
-
auto qv_and_mutex
-
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
auto qv = qv_and_mutex.first;
-
auto qv_mutex = qv_and_mutex.second;
-
-
ros_sub.StartSubscribingThread();
-
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
inekf_estimator.add_imu_propagation(
-
qimu, qimu_mutex,
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
inekf_estimator.add_velocity_correction(qv, qv_mutex,
-
"config/filter/inekf/correction/"
-
"husky_velocity_correction.yaml");
-
-
RobotStateQueuePtr robot_state_queue_ptr
-
= inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
-
= inekf_estimator.get_robot_state_queue_mutex_ptr();
-
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr,
-
robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0;
-
}
-
estimator::InekfEstimator
class for state estimation using InEKF. This class will be used to estimate the state of the robot us...
Definition: inekf_estimator.h:49
-
ros_wrapper::ROSPublisher
ROS publisher class, which publishes robot state to ROS topics including pose and path.
Definition: ros_publisher.h:49
-
ros_wrapper::ROSSubscriber
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in the queue.
Definition: ros_subscriber.h:104
-
main
int main(int argc, char **argv)
Definition: example_1.cpp:7
-
inekf_estimator.h
Header file for state estimator class.
-
RobotStateQueuePtr
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
Definition: inekf_estimator.h:39
-
estimator
Definition: inekf_estimator.cpp:16
-
state
Definition: robot_state.cpp:18
-
ros_publisher.h
Header file for ROS publisher class.
-
ros_subscriber.h
-

Let's go through the code step by step.

-

-2.1 Include necessary librarys

-

The first thing we need to do is to include necessary librarys:

#include <ros/ros.h> // ROS
-
#include <iostream> // std::cout
-
-
#include "communication/ros_publisher.h" // ROS wrapper for publisher
-
#include "communication/ros_subscriber.h" // ROS wrapper for subscriber
-
#include "estimator/inekf_estimator.h" // InEKF estimator, all the propagation and correction methods are also included in this header file
-

-2.2 Create subscribers to receive sensor data

-

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS node and a ROS node handler.

ros::init(argc, argv, "robot_state_est");
-
ros::NodeHandle nh;
-

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU data to perform propagation, we can create a subscriber to receive IMU data:

// Create a ROS subscriber class
-
ros_wrapper::ROSSubscriber ros_sub(&nh);
-
// Create a subscriber to receive IMU data
-
IMUQueuePair qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
-
// Get the pointer to IMU queue and mutex for later use
-
IMUQueuePtr qimu = qimu_and_mutex.first;
-
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
-
IMUQueuePair
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
Definition: ros_subscriber.h:50
-
IMUQueuePtr
std::shared_ptr< IMUQueue > IMUQueuePtr
Definition: ros_subscriber.h:49
-

Similarly, to add a velocity subscriber, we can do:

VelocityQueuePair qv_and_mutex = ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
-
VelocityQueuePtr qv = qv_and_mutex.first;
-
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
-
VelocityQueuePtr
std::shared_ptr< VelocityQueue > VelocityQueuePtr
Definition: ros_subscriber.h:55
-
VelocityQueuePair
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > VelocityQueuePair
Definition: ros_subscriber.h:58
-

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the DifferentialDriveVelocityCallback function in the ros_subscriber.cpp file. This function will convert joint_states messages into body velocity according to robot's kinematic model. In our case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert joint_states messages into body velocity is:

// Husky robot's kinematic model
-
double vr = (right_front_wheel_vel + right_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / 2.0 * wheel_radius;
-
double vx = (vr + vl) / 2.0; // Body velocity in x direction
-

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback function and create a new function to fit their robots' kinematic model.

-

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

ros_sub.StartSubscribingThread();
-

-2.3 Create a state estimator

-

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an InEKF estimator, we need to create a propagation method and a correction method. The propagation method is created by calling the add_<propagation_method> function in the inekf_estimator.cpp file. The correction method is created by calling the add_<correction_method> function in the inekf_estimator.cpp file. For example, if we want to use IMU data to perform propagation and use velocity data to perform correction, we can do:

// Define some config settings for the state estimator
-
inekf::ErrorType error_type = LeftInvariant;
-
YAML::Node config_ = YAML::LoadFile(
-
"config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
bool enable_imu_bias_update
-
= config_["settings"]["enable_imu_bias_update"].as<bool>();
-
-
// Create an InEKF estimator
-
InekfEstimator inekf_estimator(error_type, enable_imu_bias_update);
-
-
// Add a propagation and correction(s) methods
-
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml");
-
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml");
-
-
// Get the robot state queue and mutex from the state estimator for later use
-
RobotStateQueuePtr robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
-
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = inekf_estimator.get_robot_state_queue_mutex_ptr();
-

-2.4 Create publisher to publish state estimation results

-

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher and start the publishing thread:

// Create a ROS publisher and start the publishing thread
-
ros_wrapper::ROSPublisher ros_pub(&nh, robot_state_queue_ptr, robot_state_queue_mutex_ptr);
-
ros_pub.StartPublishingThread();
-

This publisher will publish the robot state estimation results to the topic /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be something like /robot/*/path.

-

-Step 3: Run the state estimator (InEKF version)

-

Now almost everything is settled. The last step will be running the state estimator. The state estimator should be run in a loop like the following:

// Run the state estimator
-
while (ros::ok()) {
-
// Step behavior
-
if (inekf_estimator.is_enabled()) {
-
inekf_estimator.RunOnce();
-
} else {
-
if (inekf_estimator.BiasInitialized()) {
-
inekf_estimator.InitState();
-
} else {
-
inekf_estimator.InitBias();
-
}
-
}
-
ros::spinOnce();
-
}
-
-
return 0; // Exit
-

In the loop above, we first check if the state estimator is enabled. By saying enabled, we mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the estimator is enabled, we call the RunOnce() function to perform one step of propagation and correction. If it is not enabled, we would initialize the biases and initial state according to user's settings.

-
-
- -
-Generated by doxygen 1.9.7 -
+ +
+
+
+
[ROS] State Estimator Tutorial (InEKF version, IMU + Velocity)
+
+
+
+
+

This tutorial will guide you through the process of creating a simple application using the DRIFT library. It + assumes you have a basic knowledge of C++ and Robot Operating System (ROS). If you are not familiar with ROS, + please refer to the ROS tutorials. In this tutorial, we focus + on using the InEKF version of the state estimator with IMU propagation and velocity correction.

+

+ Step 1: Edit Configs

+

There are at least two configuration files that need to be edited before running the state estimator. The + first is the <propagation>.yaml file in the + curly_state_estimator/config/filter/inekf/propagation. This file contains the settings for the + propagation method and users can copy the imu_propagation.yaml under that directory to start with + a new set of settings. The second is the <correction>.yaml file in the + curly_state_estimator/config/filter/inekf/correction. This file contains the settings for the + correction method. The velocity_correction.yaml file is a good example to start with if a user is + interested in using velocity message to perform correction. There is also a + legged_kinematics_correction.yaml file that contains the settings for the legged kinematics + correction method. All the example config files contains all the informations needed to run the state + estimator with corresponding propagation or correction method. +

+

+ Step 2: Create a new case with existing propagation and correction methods

+

Users can create a new case by following the comments in the + curly_state_estimator/ROS/curly_state_estimator/examples directory. Let's take the + husky.cpp as an example. The full file looks like: +

+
+
/* + ----------------------------------------------------------------------------
+
* Copyright 2022, CURLY Lab, University of Michigan
+
* All Rights Reserved
+
* See LICENSE for the license information
+
* + -------------------------------------------------------------------------- */
+
+
#include <ros/ros.h>
+
#include <iostream>
+
+
#include "communication/ros_publisher.h"
+
#include "communication/ros_subscriber.h"
+
#include "estimator/inekf_estimator.h"
+
+
using namespace std;
+
using namespace state;
+
using namespace estimator;
+
+
+
int main(int argc, char** argv) {
+
ros::init(argc, argv, "robot_state_est"); +
+
+
std::cout << "The subscriber is on!" + << std::endl;
+
+
ros::NodeHandle nh;
+
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
+
auto qimu_and_mutex = ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
auto qimu = qimu_and_mutex.first;
+
auto qimu_mutex = qimu_and_mutex.second;
+
+
auto qv_and_mutex
+
= ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
auto qv = qv_and_mutex.first;
+
auto qv_mutex = qv_and_mutex.second;
+
+
ros_sub.StartSubscribingThread();
+
+
inekf::ErrorType error_type = RightInvariant;
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
inekf_estimator.add_imu_propagation(
+
qimu, qimu_mutex,
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex,
+
"config/filter/inekf/correction/"
+
"husky_velocity_correction.yaml");
+
+
RobotStateQueuePtr + robot_state_queue_ptr
+
= inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr
+
= inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr,
+
robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0;
+
}
+
+
estimator::InekfEstimator
+
class for state estimation using InEKF. This class will be used to estimate the state of + the robot us...
+
Definition: inekf_estimator.h:49
+
+
+
ros_wrapper::ROSPublisher
+
ROS publisher class, which publishes robot state to ROS topics including pose and path. +
+
Definition: ros_publisher.h:49
+
+
+
ros_wrapper::ROSSubscriber
+
ROS subscriber class, which subscribes to the ROS topics and stores the measurements in + the queue.
+
Definition: ros_subscriber.h:104
+
+
+
main
+
int main(int argc, char **argv)
+
Definition: example_1.cpp:7
+
+
+
inekf_estimator.h
+
Header file for state estimator class.
+
+
+
RobotStateQueuePtr
+
std::shared_ptr< RobotStateQueue > RobotStateQueuePtr
+
Definition: inekf_estimator.h:39
+
+
+
estimator
+
Definition: inekf_estimator.cpp:16
+
+
+
state
+
Definition: robot_state.cpp:18
+
+
+
ros_publisher.h
+
Header file for ROS publisher class.
+
+
+
ros_subscriber.h
+
+
+

Let's go through the code step by step.

+

+ 2.1 Include necessary librarys

+

The first thing we need to do is to include necessary librarys:

+
+
#include <ros/ros.h> // + ROS
+
#include <iostream> // + std::cout
+
+
#include "communication/ros_publisher.h" // + ROS wrapper for publisher
+
#include "communication/ros_subscriber.h" // + ROS wrapper for subscriber
+
#include "estimator/inekf_estimator.h" // + InEKF estimator, all the propagation and correction methods are also included in this header file +
+
+

+ 2.2 Create subscribers to receive sensor data

+

The next step is to create subscribers to receive sensor data. To use ROS, we first need to initialize ROS + node and a ROS node handler.

+
+
ros::init(argc, argv, "robot_state_est");
+
ros::NodeHandle nh;
+
+

Then we can initilize the ROS subscriber wrapper and add topics to it. For example, if we want to use IMU + data to perform propagation, we can create a subscriber to receive IMU data:

+
+
// Create a ROS subscriber class
+
ros_wrapper::ROSSubscriber ros_sub(&nh);
+
// Create a subscriber to receive IMU data
+
IMUQueuePair qimu_and_mutex = + ros_sub.AddIMUSubscriber("/gx5_1/imu/data");
+
// Get the pointer to IMU queue and mutex for later use
+
IMUQueuePtr qimu = + qimu_and_mutex.first;
+
std::shared_ptr<std::mutex> qimu_mutex = qimu_and_mutex.second;
+
+
IMUQueuePair +
+
std::pair< IMUQueuePtr, std::shared_ptr< std::mutex > > IMUQueuePair
+
Definition: ros_subscriber.h:50
+
+
+
IMUQueuePtr +
+
std::shared_ptr< IMUQueue > IMUQueuePtr
+
Definition: ros_subscriber.h:49
+
+
+

Similarly, to add a velocity subscriber, we can do:

+
+
VelocityQueuePair qv_and_mutex = + ros_sub.AddDifferentialDriveVelocitySubscriber("/joint_states");
+
VelocityQueuePtr qv = + qv_and_mutex.first;
+
std::shared_ptr<std::mutex> qv_mutex = qv_and_mutex.second;
+
+
VelocityQueuePtr +
+
std::shared_ptr< VelocityQueue > VelocityQueuePtr
+
Definition: ros_subscriber.h:55
+
+
+
VelocityQueuePair
+
std::pair< VelocityQueuePtr, std::shared_ptr< std::mutex > > + VelocityQueuePair
+
Definition: ros_subscriber.h:58
+
+
+

The AddDifferentialDriveVelocitySubscriber(<topic>) function above will call the + DifferentialDriveVelocityCallback function in the + ros_subscriber.cpp file. This function will + convert joint_states messages into body velocity according to robot's kinematic model. In our + case, Clearpath Husky robot is a four-wheel-drived robot and the formula to convert + joint_states messages into body velocity is: +

+
+
// Husky robot's kinematic model
+
double vr = (right_front_wheel_vel + right_rear_wheel_vel) + / 2.0 * wheel_radius;
+
double vl = (left_front_wheel_vel + left_rear_wheel_vel) / + 2.0 * wheel_radius;
+
double vx = (vr + vl) / 2.0; // Body + velocity in x direction
+
+

If users want to use other robots, they can follow the DifferentialDriveVelocityCallback + function and create a new function to fit their robots' kinematic model.

+

After adding all the subscribers, we can start the subscriber thread to avoid traffic jam:

+
+
ros_sub.StartSubscribingThread();
+
+

+ 2.3 Create a state estimator

+

The next step is to create a state estimator. In this example, we will use the InEKF estimator. To create an + InEKF estimator, we need to create a propagation method and a correction method. The propagation method is + created by calling the add_<propagation_method> function in the + inekf_estimator.cpp + file. The correction method is created by calling the add_<correction_method> function in + the + inekf_estimator.cpp + file. For example, if we want to use IMU data to perform propagation and use velocity data to perform + correction, we can do: +

+
+
// Define some config settings for the state estimator
+
inekf::ErrorType error_type = LeftInvariant;
+
YAML::Node config_ = YAML::LoadFile(
+
"config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
bool enable_imu_bias_update
+
= config_["settings"]["enable_imu_bias_update"].as<bool>();
+
+
// Create an InEKF estimator
+
InekfEstimator + inekf_estimator(error_type, enable_imu_bias_update);
+
+
// Add a propagation and correction(s) methods
+
inekf_estimator.add_imu_propagation(qimu, qimu_mutex, "config/filter/inekf/propagation/husky_imu_propagation.yaml"); +
+
inekf_estimator.add_velocity_correction(qv, qv_mutex, "config/filter/inekf/correction/husky_velocity_correction.yaml"); +
+
+
// Get the robot state queue and mutex from the state estimator for + later use
+
RobotStateQueuePtr + robot_state_queue_ptr = inekf_estimator.get_robot_state_queue_ptr();
+
std::shared_ptr<std::mutex> robot_state_queue_mutex_ptr = + inekf_estimator.get_robot_state_queue_mutex_ptr();
+
+

+ 2.4 Create publisher to publish state estimation results

+

We have a ROS publisher wrapper to publish state estimation results. To use it, we need to create a publisher + and start the publishing thread:

+
+
// Create a ROS publisher and start the publishing thread
+
ros_wrapper::ROSPublisher ros_pub(&nh, + robot_state_queue_ptr, robot_state_queue_mutex_ptr);
+
ros_pub.StartPublishingThread();
+
+

This publisher will publish the robot state estimation results to the topic + /robot/<robot_name>/state. Users can use RVIZ to visualize the path. The path topic will be + something like /robot/*/path. +

+

+ Step 3: Run the state estimator (InEKF version)

+

Now almost everything is settled. The last step will be running the state estimator. The state estimator + should be run in a loop like the following:

+
+
// Run the state estimator
+
while (ros::ok()) {
+
// Step behavior
+
if (inekf_estimator.is_enabled()) {
+
inekf_estimator.RunOnce();
+
} else {
+
if (inekf_estimator.BiasInitialized()) {
+
inekf_estimator.InitState();
+
} else {
+
inekf_estimator.InitBias();
+
}
+
}
+
ros::spinOnce();
+
}
+
+
return 0; // Exit
+
+

In the loop above, we first check if the state estimator is enabled. By saying enabled, we + mean if the necessary biases and initial state are initialized so that the estimator is ready to run. If the + estimator is enabled, we call the RunOnce() function to perform one step of propagation and + correction. If it is not enabled, we would initialize the biases and initial state according to user's + settings.

+
+
+
+ +
+
+ Generated by doxygen 1.9.7 +
- + + \ No newline at end of file