From 5dc7993f1838d175ddf28f6884b71259ea0d4bfe Mon Sep 17 00:00:00 2001
From: mingxi <mzhou@uri.edu>
Date: Thu, 2 May 2024 09:29:27 -0400
Subject: [PATCH] changed tf lookup timneout to get the most recent one in
 f_close_loop relatied functions. Otherwise the controller frequency will be
 slow

---
 src/mvp_control/mvp_control_ros.cpp | 20 ++++++++------------
 1 file changed, 8 insertions(+), 12 deletions(-)

diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp
index 6ffc4a1..80b6731 100644
--- a/src/mvp_control/mvp_control_ros.cpp
+++ b/src/mvp_control/mvp_control_ros.cpp
@@ -489,10 +489,9 @@ bool MvpControlROS::f_update_control_allocation_matrix() {
         auto cg_world = m_transform_buffer.lookupTransform(
             m_world_link_id,
             m_cg_link_id,
-            ros::Time::now(),
-            ros::Duration(15.0)
+            ros::Time(0)
         );
-
+        
         auto tf_eigen = tf2::transformToEigen(cg_world);
 
         tf2::Quaternion quat;
@@ -573,8 +572,7 @@ bool MvpControlROS::f_compute_process_values() {
         auto cg_world = m_transform_buffer.lookupTransform(
             m_world_link_id,
             m_cg_link_id,
-            ros::Time::now(),
-            ros::Duration(10.0)
+            ros::Time(0)
         );
 
         tf2::Quaternion quat;
@@ -599,13 +597,12 @@ bool MvpControlROS::f_compute_process_values() {
     }
         // Transform from odom to world
     try{
-        std::scoped_lock lock(m_odom_lock);
+        // std::scoped_lock lock(m_odom_lock);
 
         auto cg_odom = m_transform_buffer.lookupTransform(
                 m_cg_link_id,
                 m_odometry_msg.child_frame_id,
-                ros::Time::now(),
-                ros::Duration(10)
+                ros::Time(0)
         );
 
         auto cg_odom_eigen = tf2::transformToEigen(cg_odom);
@@ -697,6 +694,7 @@ bool MvpControlROS::f_compute_process_values() {
     e.angular_rate.z = error_state(DOF::YAW_RATE);
 
     m_process_error_publisher.publish(e);
+    // printf("end dt = %lf\r\n", ros::Time::now().toSec() - pt);
 
     return true;
 }
@@ -1344,8 +1342,7 @@ bool MvpControlROS::f_amend_set_point(
         auto tf_world_setpoint = m_transform_buffer.lookupTransform(
             m_world_link_id,
             set_point.header.frame_id,
-            ros::Time::now(),
-            ros::Duration(10.0)
+            ros::Time(0)
         );
 
         auto tf_eigen = tf2::transformToEigen(tf_world_setpoint);
@@ -1363,8 +1360,7 @@ bool MvpControlROS::f_amend_set_point(
         auto tf_1 = m_transform_buffer.lookupTransform(
             set_point.header.frame_id,
             m_world_link_id,
-            ros::Time::now(),
-            ros::Duration(10.0)
+            ros::Time(0)
         );
         auto tf_1_eigen = tf2::transformToEigen(tf_1);
         //find the total rotation matrix from the world link to the desired pose.