modify odometry_new

catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp

@@ -120,11 +120,15 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
     
     // Covariance Matrix initialization
     // Pose Covariance
-    for (int i = 0; i < 3; ++i) {
-        for (int j = 0; j < 3; ++j) {
-            odom.pose.covariance[i * 6 + j] = pose_covariance(i, j);
-        }
-    }
+    odom.pose.covariance[0] = pose_covariance(0, 0); // x-x
+    odom.pose.covariance[1] = pose_covariance(0, 1); // x-y
+    odom.pose.covariance[5] = pose_covariance(0, 2); // x-yaw
+    odom.pose.covariance[6] = pose_covariance(1, 0); // y-x
+    odom.pose.covariance[7] = pose_covariance(1, 1); // y-y
+    odom.pose.covariance[11] = pose_covariance(1, 2); // y-yaw
+    odom.pose.covariance[30] = pose_covariance(2, 0); // yaw-x
+    odom.pose.covariance[31] = pose_covariance(2, 1); // yaw-y
+    odom.pose.covariance[35] = pose_covariance(2, 2); // yaw-yaw
 
     // Twist covariance
     for (int i = 0; i < 36; ++i) {
@@ -166,8 +170,6 @@ int main(int argc, char **argv) {
 
     m_subscriber = nh.subscribe("/asc/encoder_counts", 1, answer);
     pwm_subscriber = nh.subscribe("/asc/current_motor_duty_cycle", 1, pwmAnswer);
-
-    m_publisher = nh.advertise<asclinic_pkg::position>("/current_position", 10);
     m_odometry_publisher = nh.advertise<nav_msgs::Odometry>("/odom", 10);
 
     ROS_INFO_STREAM("Node started");