From 82d9ca4759d551a1bf4dc34d1621d405f8e42409 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Tue, 1 Mar 2022 14:28:46 +0100
Subject: [PATCH] testing
---
src/subscriber_imu.cpp | 9 +++++++--
1 file changed, 7 insertions(+), 2 deletions(-)
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 22d5ff5..a3a32ef 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -325,7 +325,12 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
{
- slope = 0;
+ auto q_frame = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
+ auto g_local_xy = (q_frame.conjugate() * gravity()).head<2>();
+ slope = asin(g_local_xy.norm() / gravity().norm());
+ orientation = atan2(g_local_xy(1),g_local_xy(0));
+
+ /*slope = 0;
auto n_frames = 0;
for (auto frm_it : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
{
@@ -339,7 +344,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
n_frames++;
}
- slope /= n_frames;
+ slope /= n_frames;*/
}
else
return;
--
GitLab