base odom publisher was added

amirhosein-vedadi · Aug 26, 2022 · aeb9db7 · aeb9db7
1 parent 584f2d3
commit aeb9db7
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 1 deletion.
diff --git a/trajectory_planner/include/trajectory_planner/LieEKF.h b/trajectory_planner/include/trajectory_planner/LieEKF.h
@@ -42,6 +42,9 @@ class LieEKF {
 
         void runFilter(Vector3d gyro, Vector3d acc, Vector3d lfpmeasured, Vector3d rfpmeasured, Matrix3d lfrot, Matrix3d rfrot, int* contact, bool update_enaled);
 
+        Vector3d getGBasePose();
+        Vector3d getGBaseVel();
+        Quaterniond getGBaseQuat();
 
     private:
         Vector3d GLeftFootPos_;

diff --git a/trajectory_planner/include/trajectory_planner/QuatEKF.h b/trajectory_planner/include/trajectory_planner/QuatEKF.h
@@ -43,7 +43,10 @@ class QuatEKF {
 
         void runFilter(Vector3d gyro, Vector3d acc, Vector3d lfpmeasured, Vector3d rfpmeasured, Matrix3d lfrot, Matrix3d rfrot, int* contact, bool update_enaled);
 
-
+        Vector3d getGBasePose();
+        Vector3d getGBaseVel();
+        Quaterniond getGBaseQuat();
+
     private:
         Vector3d GLeftFootPos_;
         Vector3d GRightFootPos_;

diff --git a/trajectory_planner/src/LieEKF.cpp b/trajectory_planner/src/LieEKF.cpp
@@ -52,6 +52,19 @@ LieEKF::LieEKF(){
 
 LieEKF::~LieEKF(){}
 
+Vector3d LieEKF::getGBasePose(){
+    return GBasePos_;
+}
+
+Vector3d LieEKF::getGBaseVel(){
+    return GBaseVel_;
+}
+
+Quaterniond LieEKF::getGBaseQuat(){
+    Quaterniond q(GBaseRot_);
+    return q;
+}
+
 void LieEKF::initializeStates(Matrix3d R, Vector3d base_vel, Vector3d base_pos, Vector3d lf_pos, 
                               Vector3d rf_pos, Vector3d gyro_bias, Vector3d acc_bias){
     GBaseRot_ = R;

diff --git a/trajectory_planner/src/QuatEKF.cpp b/trajectory_planner/src/QuatEKF.cpp
@@ -66,6 +66,18 @@ QuatEKF::~QuatEKF() {
 
 }
 
+Vector3d QuatEKF::getGBasePose(){
+    return GBasePos_;
+}
+
+Vector3d QuatEKF::getGBaseVel(){
+    return GBaseVel_;
+}
+
+Quaterniond QuatEKF::getGBaseQuat(){
+    return GBaseQuat_;
+}
+
 void QuatEKF::initializeStates(Quaterniond q, Vector3d base_vel, Vector3d base_pos, Vector3d lf_pos, 
                                Vector3d rf_pos, Vector3d gyro_bias, Vector3d acc_bias){
     GBaseQuat_ = q;