Merge branch 'master' of github.com:ctu-mrs/mrs_vins_republisher

ctu-mrs · Dec 19, 2024 · 03aa46f · 03aa46f
2 parents ab06d5f + 971d272
commit 03aa46f
Showing 1 changed file with 10 additions and 9 deletions.
diff --git a/src/VinsRepublisher.cpp b/src/VinsRepublisher.cpp
@@ -75,7 +75,6 @@ class VinsRepublisher : public nodelet::Nodelet {
   bool            is_averaging_finished_ = false;
   int             n_imu_meas_            = 0;
   Eigen::Vector3d mean_acc_;
-  Eigen::Matrix3d init_rot_;
 
   ros::Publisher publisher_odom_;
   ros::Time      publisher_odom_last_published_;
@@ -137,7 +136,7 @@ void VinsRepublisher::onInit() {
 
   param_loader.loadParam("init_in_zero", _init_in_zero_, true);
 
-  param_loader.loadParam("compensate_initial_tilt_", compensate_initial_tilt_, false);
+  param_loader.loadParam("compensate_initial_tilt", compensate_initial_tilt_, false);
 
   if (!param_loader.loadedSuccessfully()) {
     ROS_ERROR("[%s]: parameter loading failure", node_name.c_str());
@@ -410,17 +409,18 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
     if (is_calibrated_) {
 
       // First subtract the initial position in the initialization frame
-      odom_transformed.pose.pose.position.x =- odom_init_.pose.pose.position.x;
-      odom_transformed.pose.pose.position.y =- odom_init_.pose.pose.position.y;
-      odom_transformed.pose.pose.position.z =- odom_init_.pose.pose.position.z;
+      odom_transformed.pose.pose.position.x -= odom_init_.pose.pose.position.x;
+      odom_transformed.pose.pose.position.y -= odom_init_.pose.pose.position.y;
+      odom_transformed.pose.pose.position.z -= odom_init_.pose.pose.position.z;
 
       // Rotate position to the calibrated frame
       Eigen::Vector3d pos;
+      Eigen::Matrix3d init_rot = mrs_lib::AttitudeConverter(odom_init_.pose.pose.orientation);
       pos << odom_transformed.pose.pose.position.x, odom_transformed.pose.pose.position.y, odom_transformed.pose.pose.position.z;
-      pos = init_rot_.inverse() * pos;
+      pos = init_rot.inverse() * pos;
 
       // Unrotate orientation
-      const Eigen::Quaterniond q_init_rot = mrs_lib::AttitudeConverter(init_rot_);
+      const Eigen::Quaterniond q_init_rot = mrs_lib::AttitudeConverter(init_rot);
       const Eigen::Quaterniond q_curr_rot = mrs_lib::AttitudeConverter(odom_transformed.pose.pose.orientation);
       const Eigen::Quaterniond q_calibrated = q_init_rot * q_curr_rot;
 
@@ -429,7 +429,8 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
     } else {
         mrs_lib::set_mutexed(mtx_odom_init_, odom_transformed, odom_init_);
         auto [roll, pitch, yaw] = mrs_lib::AttitudeConverter(odom_init_.pose.pose.orientation).getExtrinsicRPY();
-        ROS_INFO_THROTTLE(1.0, "[VinsRepublisher]: init_odom: t: (%.2f, %.2f, %.2f) [m] rpy: (%.2f, %.f, %.2f) [deg], waiting for calibration service call", odom_init_.pose.pose.position.x, odom_init_.pose.pose.position.y, odom_init_.pose.pose.position.z, roll, pitch, yaw);
+        ROS_INFO_THROTTLE(1.0, "[VinsRepublisher]: init_odom: t: (%.2f, %.2f, %.2f) [m] rpy: (%.2f, %.f, %.2f) [deg], waiting for calibration service call", odom_init_.pose.pose.position.x, odom_init_.pose.pose.position.y, odom_init_.pose.pose.position.z, roll*180/3.14, pitch*180/3.14, yaw*180/3.14);
+        has_valid_odom_ = true;
         return;
     }
   }
@@ -534,7 +535,7 @@ bool VinsRepublisher::calibrateSrvCallback(std_srvs::SetBool::Request &req, std_
 
   ROS_INFO("[%s]: calibrating level horizon.", getName().c_str());
   auto [roll, pitch, yaw] = mrs_lib::AttitudeConverter(odom_init_.pose.pose.orientation).getExtrinsicRPY();
-  ROS_INFO_THROTTLE(1.0, "[VinsRepublisher]: calibrated initial pose as: t: (%.2f, %.2f, %.2f) [m] rpy: (%.2f, %.f, %.2f) [deg]", odom_init_.pose.pose.position.x, odom_init_.pose.pose.position.y, odom_init_.pose.pose.position.z, roll, pitch, yaw);
+  ROS_INFO_THROTTLE(1.0, "[VinsRepublisher]: calibrated initial pose as: t: (%.2f, %.2f, %.2f) [m] rpy: (%.2f, %.f, %.2f) [deg]", odom_init_.pose.pose.position.x, odom_init_.pose.pose.position.y, odom_init_.pose.pose.position.z, roll*180/3.14, pitch*180/3.14, yaw*180/3.14);
 
   res.success = true;
   res.message = "calibrated";