odometry_new update

catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp

@@ -14,12 +14,32 @@ double time_float; // Floating-point representation of elapsed time
 constexpr float WHEEL_RADIUS_LEFT = 0.072;  // Left wheel radius in meters
 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
+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; // Publisher for angular velocity as a single float
 ros::Publisher m_odometry_publisher; // Publisher for odometry
 
+float normalize_angle(float angle) {
+    if (angle >= 0) {
+        return fmod(angle, 2 * M_PI);
+    } else {
+        float value = fmod(angle, 2 * M_PI);
+        return 2 * M_PI - value;
+    }
+}
+
 
 void answer(const asclinic_pkg::LeftRightInt32& msg) {
     // Update times for elapsed time calculation
@@ -41,7 +61,28 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
     x += dS * cos(phi + 0.5 * dPhi);
     y += dS * sin(phi + 0.5 * dPhi);
     phi += dPhi;
+    phi = normalize_angle(phi);
+    
+    // Compute Covariance
+    MatrixXd jacob_p(3, 3);
+    jacob_p << 1, 0, -dS * sin(phi + 0.5 * dPhi),
+               0, 1, dS * cos(phi + 0.5 * dPhi),
+               0, 0, 1;
 
+    MatrixXd theta_covariance(2, 2);
+    theta_covariance << k_l * std::abs(deltaThetaL), 0,
+                     	 0, k_r * std::abs(deltaThetaR);
+
+    float r = WHEEL_RADIUS_RIGHT; // Moved this declaration here for consistency
+
+    MatrixXd jacob_phi(3, 2);
+    jacob_phi << r/2 * cos(phi + 0.5 * dPhi) + r/4 / HALF_WHEEL_BASE * dS * sin(phi + 0.5 * dPhi), r/2 * cos(phi + 0.5 * dPhi) - r/4 / HALF_WHEEL_BASE * dS * sin(phi + 0.5 * dPhi),
+    r/2 * sin(phi + 0.5 * dPhi) - r/4 / HALF_WHEEL_BASE * dS * cos(phi + 0.5 * dPhi), r/2 * sin(phi + 0.5 * dPhi) + r/4 / HALF_WHEEL_BASE * dS * cos(phi + 0.5 * dPhi),
+    -r / (2 * HALF_WHEEL_BASE), r / (2 * HALF_WHEEL_BASE);
+
+    MatrixXd pose_covariance_update = jacob_p * pose_covariance * jacob_p.transpose() + jacob_phi * theta_covariance * jacob_phi.transpose();
+    pose_covariance = pose_covariance_update;
+    
     // Calculate angular velocity
     float angular_velocity = dPhi / time_float;
 
@@ -79,40 +120,141 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
     
     // Covariance Matrix initialization
     // Pose Covariance
-    for (int i = 0; i < 36; ++i) {
-        if (i == 0 || i == 7 || i == 14) { // Covariance for x, y, z positions
-            odom.pose.covariance[i] = 0.01;
-        } else if (i == 21 || i == 28 || i == 35) { // Covariance for roll, pitch, yaw
-            odom.pose.covariance[i] = 0.1;
-        } else {
-            odom.pose.covariance[i] = 0.0;
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; ++j) {
+            odom.pose.covariance[i * 6 + j] = pose_covariance(i, j);
         }
     }
-    // Twist Covariance
+
+    // Twist covariance
     for (int i = 0; i < 36; ++i) {
-        if (i == 0 || i == 7 || i == 14) { // Covariance for linear velocities
-            odom.twist.covariance[i] = 0.01;
-        } else if (i == 21 || i == 28 || i == 35) { // Covariance for angular velocities
-            odom.twist.covariance[i] = 0.1;
-        } else {
-            odom.twist.covariance[i] = 0.0;
-        }
+        odom.twist.covariance[i] = 0.1;
     }
     m_odometry_publisher.publish(odom);
 }
 
-int main(int argc, char **argv) {
-    ros::init(argc, argv, "receive_encoder");
+void ImageAnswer(const asclinic_pkg::position& msg) {
+    constrain_time = ros::Time::now();
+    constrain_duration = constrain_time - begin_time;
+    float temp_value = constrain_duration.toSec();
+    if (temp_value < 50) {
+        return;
+    }
+    // Check if elapsed time is less than 0.5 seconds
+    if (authority ==0) {
+        return;
+    }
+
+    // Update the last time to the current time
+
+    MatrixXd z_position(3, 1);
+    z_position << msg.x,
+                  msg.y,
+                  msg.phi;
+
+    MatrixXd K = pose_covariance * (pose_covariance + z_cov).inverse();
+    MatrixXd odo_position(3, 1);
+    odo_position << x,
+                    y,
+                    phi;
+
+    //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);
 
+    if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4||std::abs(odo_position(0)-new_pose(0))>1.5||std::abs(odo_position(1)-new_pose(1)>1.5)) {
+        x = odo_position(0);
+        y = odo_position(1);
+        phi = odo_position(2);
+        ROS_WARN("Fail: new_pose contains NaN values");
+    } else {
+        x = new_pose(0);
+        y = new_pose(1);
+        phi = new_pose(2);
+        pose_covariance = pose_covariance - K * (pose_covariance + z_cov) * K.transpose();
+        ROS_INFO_STREAM("Updated pose: X:" << x << ", Y:" << y << ", Phi:" << phi);
+    }
+
+    phi = normalize_angle(phi);
+    //ROS_INFO_STREAM("Normalized Phi: " << phi);
+
+    ROS_INFO("Image updated");
+    authority = 0;
+
+    std_msgs::UInt32 Umsg;
+    Umsg.data = 1;
+    kalman_output.publish(Umsg);
+}
+
+
+void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
+    if(msg.left<0){
+        left_p = -1;
+    }
+    else{
+        left_p = 1;
+    }
+    if(msg.right<0){
+        right_p = -1;
+    }
+    else{
+        right_p = 1;
+    }
+}
+
+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");
+        }
+        
+        ros::spinOnce();
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "Kalman");
     std::string ns_for_group = ros::this_node::getNamespace();
     ros::NodeHandle nh(ns_for_group);
-
+    begin_time = ros::Time::now();
     // Initialize time_now to current time
     time_now = ros::Time::now();
 
-    m_subscriber = nh.subscribe("asc/encoder_counts", 1, answer);
-    m_publisher = nh.advertise<asclinic_pkg::position>("current_position", 1);
-    m_angular_velocity_publisher = nh.advertise<asclinic_pkg::vw>("angular_velocity_topic", 1);
+    pose_covariance << 1, 1, 1,
+                       1, 1, 1,
+                       1, 1, 1;
+
+    z_cov << 10, 10, 10,
+             10, 10, 10,
+             10, 10, 10;
+
+    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);
+    m_odometry_publisher = nh.advertise<nav_msgs/Odometry>("/odom", 10);
 
     ROS_INFO_STREAM("Node started");
     ros::spin();