fixed trajectory tracking for vel and pos modality

ctu-mrs · Feb 14, 2024 · 8704ccb · 8704ccb
1 parent 1d0f3eb
commit 8704ccb
Showing 1 changed file with 13 additions and 3 deletions.
diff --git a/src/mpc_tracker/mpc_tracker.cpp b/src/mpc_tracker/mpc_tracker.cpp
@@ -312,6 +312,7 @@ class MpcTracker : public mrs_uav_managers::Tracker {
 
   ros::Timer        timer_mpc_iteration_;
   std::atomic<bool> mpc_synchronous_ = false;
+  ros::TimerEvent   synchronous_timer_event_;
 
   std::atomic<bool> mpc_timer_running_ = false;
   void              timerMPC(const ros::TimerEvent& event);
@@ -1001,11 +1002,20 @@ std::optional<mrs_msgs::TrackerCommand> MpcTracker::update(const mrs_msgs::UavSt
     return {tracker_cmd};
   }
 
-  ros::TimerEvent event;
-
   if (mpc_synchronous_) {
+
     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
-    timerMPC(event);
+
+    if (synchronous_timer_event_.last_real.toSec() > 0) {
+      synchronous_timer_event_.last_real = synchronous_timer_event_.current_real;
+    } else {
+      synchronous_timer_event_.last_real = ros::Time::now();
+    }
+
+    synchronous_timer_event_.current_real = ros::Time::now();
+
+    timerMPC(synchronous_timer_event_);
+
   } else {
     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
   }