From 74c035a7b5e36be9276310c5f7e7bd266ca7122a Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Wed, 24 Jul 2024 12:57:54 +0800
Subject: [PATCH] modify odometry_new
---
.../asclinic_pkg/src/forklift/odometry_new.cpp | 16 +++++++++-------
1 file changed, 9 insertions(+), 7 deletions(-)
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
index 22f38dec..13b86b43 100644
--- a/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
+++ b/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");
--
GitLab