kalman only after rotating

catkin_ws/src/asclinic_pkg/src/Kalman.cpp

@@ -9,6 +9,7 @@
 #include <eigen3/Eigen/Dense>
 #include "asclinic_pkg/vw.h"
 #include <std_msgs/UInt32.h>
+#include <thread>
 
 using Eigen::MatrixXd;
 
@@ -18,7 +19,7 @@ float x = -1, y = -3, phi = M_PI/2;
 int left_p = 1, right_p = 1;
 ros::Time time_previous, time_now; // Timing variables
 ros::Duration elapsed_time; // Duration to calculate elapsed time
-ros::Time last_time;
+
 
 double time_float; // Floating-point representation of elapsed time
 
@@ -140,13 +141,13 @@ void ImageAnswer(const asclinic_pkg::position& msg) {
                     y,
                     phi;
 
-    ROS_INFO_STREAM("odo_position: " << odo_position);
-    ROS_INFO_STREAM("z_position: " << z_position);
-    ROS_INFO_STREAM("K: " << K);
+    //ROS_INFO_STREAM("odo_position: " << odo_position);
+    //ROS_INFO_STREAM("z_position: " << z_position);
+    //ROS_INFO_STREAM("K: " << K);
 
     MatrixXd new_pose = odo_position + K * (z_position - odo_position);
 
-    ROS_INFO_STREAM("new_pose: " << new_pose);
+    //ROS_INFO_STREAM("new_pose: " << new_pose);
 
     if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))) {
         x = odo_position(0);
@@ -162,7 +163,7 @@ void ImageAnswer(const asclinic_pkg::position& msg) {
     }
 
     phi = normalize_angle(phi);
-    ROS_INFO_STREAM("Normalized Phi: " << phi);
+    //ROS_INFO_STREAM("Normalized Phi: " << phi);
 
     ROS_INFO("Image updated");
     authority = 0;
@@ -190,7 +191,26 @@ void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
 
 void update_authority(const std_msgs::UInt32& msg) {
     authority = 1;
+    ros::Time last_time = ros::Time::now();
+    ros::Time current_time = ros::Time::now();
+    ros::Duration lastest_duration;
+    float duration_authority;
     ROS_INFO_STREAM("Authority updated to: " << authority);
+    while(authority){
+        current_time = ros::Time::now();
+        lastest_duration = current_time - last_time;
+        duration_authority = lastest_duration.toSec();
+        if(duration_authority > 2){
+            authority = 0;
+            std_msgs::UInt32 Umsg;
+            Umsg.data = 1;
+            kalman_output.publish(Umsg);
+            ROS_INFO_STREAM("go with no update");
+        }
+        last_time = current_time;
+        ros::spinOnce();
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
 }
 
 
@@ -201,7 +221,7 @@ int main(int argc, char **argv) {
 
     // Initialize time_now to current time
     time_now = ros::Time::now();
-    last_time = ros::Time::now();
+    
     
     pose_covariance <<  0.5, 0.5, 0.5,
                         0.5, 0.5, 0.5,

catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp

@@ -32,7 +32,6 @@ public:
     }
 };
 
-void rotation_thread_function();
 
 PIDController pid(2, 0, 5);
 PIDController pid1(40, 0, 0);
@@ -151,7 +150,7 @@ void purerotation(const asclinic_pkg::rotation& rotation_msg) {
 }
 
 void kalman_change(const std_msgs::UInt32& Umsg){
-    
+
     // Publish state message
     std_msgs::UInt32 state_msg;
     state_msg.data = 1;