new logic

catkin_ws/src/asclinic_pkg/src/Kalman.cpp

@@ -8,6 +8,7 @@
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
 #include "asclinic_pkg/vw.h"
+#include <std_msgs/UInt32.h>
 
 using Eigen::MatrixXd;
 
@@ -27,6 +28,8 @@ constexpr float WHEEL_RADIUS_RIGHT = 0.072; // Right wheel radius in meters
 constexpr float HALF_WHEEL_BASE = 0.125;    // Half of the wheelbase in meters
 constexpr float k_l = 0.00023;  // Uncertainty in the left wheel
 constexpr float k_r = 0.0001;  // Uncertainty in the right wheel
+int authority =0;
+
 
 // Covariance matrix for pose
 MatrixXd pose_covariance(3, 3); // Initialize a 3x3 matrix for pose covariance
@@ -35,10 +38,11 @@ MatrixXd z_cov(3, 3);
 ros::Subscriber m_subscriber;
 ros::Subscriber image_subscriber;
 ros::Subscriber pwm_subscriber;
+ros::Subscriber kalman_subscriber;
 
 ros::Publisher m_publisher;
 ros::Publisher m_angular_velocity_publisher;
-
+ros::Publisher kalman_output;
 
 
 float normalize_angle(float angle) {
@@ -117,15 +121,13 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
 
 
 void ImageAnswer(const asclinic_pkg::position& msg) {
-    ros::Time current_time = ros::Time::now();
 
     // Check if elapsed time is less than 0.5 seconds
-    if ((current_time - last_time).toSec() < 8) {
+    if (authority ==0) {
         return;
     }
 
     // Update the last time to the current time
-    last_time = current_time;
 
     MatrixXd z_position(3, 1);
     z_position << msg.x,
@@ -163,6 +165,11 @@ void ImageAnswer(const asclinic_pkg::position& msg) {
     ROS_INFO_STREAM("Normalized Phi: " << phi);
 
     ROS_INFO("Image updated");
+    authority = 0;
+
+    std_msgs::UInt32 Umsg;
+    Umsg.data = 1;
+    kalman_output.publish(Umsg);
 }
 
 
@@ -181,6 +188,12 @@ void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
     }
 }
 
+void update_authority(const std_msgs::UInt32& msg) {
+    authority = 1;
+    ROS_INFO_STREAM("Authority updated to: " << authority);
+}
+
+
 int main(int argc, char **argv) {
     ros::init(argc, argv, "Kalman");
     std::string ns_for_group = ros::this_node::getNamespace();
@@ -190,21 +203,22 @@ int main(int argc, char **argv) {
     time_now = ros::Time::now();
     last_time = ros::Time::now();
     
-    pose_covariance <<  1, 1, 1,
-                        1, 1, 1,
-                        1, 1, 1;
+    pose_covariance <<  0.5, 0.5, 0.5,
+                        0.5, 0.5, 0.5,
+                        0.5, 0.5, 0.5;
 
-    z_cov <<  0.07246, 0.05582, 0.01839,
-              0.05582, 0.32237, 0.46697,
-              0.01839, 0.46697, 0.20327;
+    z_cov <<  5, 5, 5,
+              5, 5, 5,
+              5, 5, 5;
 
     m_subscriber = nh.subscribe("/asc/encoder_counts", 1, answer);
     image_subscriber = nh.subscribe("/image_estimation", 1, ImageAnswer);
     pwm_subscriber = nh.subscribe("/asc/current_motor_duty_cycle",1, pwmAnswer);
-
+    kalman_subscriber = nh.subscribe("/kalman_demand",1, update_authority);
 
     m_publisher = nh.advertise<asclinic_pkg::position>("/current_position", 10);
     m_angular_velocity_publisher = nh.advertise<asclinic_pkg::vw>("/angular_velocity_topic", 10);
+    kalman_output = nh.advertise<std_msgs::UInt32>("/kalman_info", 1);
     ROS_INFO_STREAM("Node started");
     ros::spin();
 

catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp

@@ -42,8 +42,11 @@ ros::Subscriber vw_sub;
 ros::Subscriber vw_r_sub;
 ros::Subscriber odometry_sub;
 ros::Subscriber position_sub;
+ros::Subscriber kalman_back;
+
 ros::Publisher output_pub;
 ros::Publisher state_pub;
+ros::Publisher kalman_request;
 
 std::mutex mtx;
 
@@ -139,6 +142,16 @@ void purerotation(const asclinic_pkg::rotation& rotation_msg) {
     output_pub.publish(output_msg);
     ROS_INFO_STREAM("Motors stopped after rotation");
 
+    std_msgs::UInt32 Umsg;
+    Umsg.data = 1;
+    kalman_request.publish(Umsg);
+
+
+    
+}
+
+void kalman_change(const std_msgs::UInt32& Umsg){
+    
     // Publish state message
     std_msgs::UInt32 state_msg;
     state_msg.data = 1;
@@ -177,6 +190,8 @@ void updateO(const asclinic_pkg::vw& w_msg) {
     output_msg.left = left;
     output_msg.right = right;
     output_pub.publish(output_msg);
+
+
     
 }
 void updatep(const asclinic_pkg::position& msg){
@@ -195,9 +210,11 @@ int main(int argc, char** argv) {
     vw_r_sub = nh.subscribe("/vw_r_information", 3, purerotation);
     odometry_sub = nh.subscribe("/angular_velocity_topic", 1, updateO);
     position_sub = nh.subscribe("/current_position", 1, updatep);
+    kalman_back = nh.subscribe("/kalman_info", 1, kalman_change);
 
     output_pub = nh.advertise<asclinic_pkg::LeftRightFloat32>("/asc/set_motor_duty_cycle", 1, false);
     state_pub = nh.advertise<std_msgs::UInt32>("/controller1_state", 3, false);
+    kalman_request = nh.advertise<std_msgs::UInt32>("/kalman_demand", 1, false);
     ros::spin();
 
     return 0;