ekf_odom_pub.cpp 수정사항

## 주요 수정 내용 설명

### 1. 초기 위치 수신 문제 해결

문제: if(initialPoseRecieved) 조건문 때문에 RViz에서 초기 위치를 수동으로 지정해주기 전까지 오도메트리 계산이 시작되지 않았습니다.
해결: main 함수의 while 루프에서 해당 조건문을 완전히 제거했습니다. 이제 노드가 실행되면 즉시 (0,0,0) 위치에서 오도메트리 계산을 시작합니다. initial_2d 토픽은 중간에 위치를 보정하는 용도로만 사용됩니다.

### 2. TF 발행 기능 추가

문제: 계산된 odom -> base_footprint 관계를 TF로 발행하지 않아 다른 노드(AMCL, RViz 등)가 로봇의 위치를 알 수 없었습니다.
해결:
1. tf2_ros/transform_broadcaster.h와 geometry_msgs/TransformStamped.h를 include 했습니다.
2. publish_quat_and_tf 함수 내부에 TransformBroadcaster를 생성하고, 계산된 위치와 방향 정보로 TransformStamped 메시지를 채운 뒤 br.sendTransform()을 통해 TF를 발행하도록 했습니다. 📢

### 3. Covariance 계산 버그 수정

문제: 공분산(covariance) 값을 += 연산자로 계속 더하여 불확실성이 비정상적으로 증가했습니다.
해결: publish_quat_and_tf 함수에서 = 연산자를 사용하여 정확한 고정값을 할당하도록 수정했습니다. 사용하지 않는 Z, Roll, Pitch 축의 공분산은 매우 큰 값(1e6)을 넣어 필터가 해당 값을 무시하도록 설정했습니다.

### 4. 기타 개선 사항

파라미터 처리: 바퀴 반지름(WHEEL_RADIUS)과 바퀴 간 거리(WHEEL_BASE)를 코드에 하드코딩하는 대신, ROS 파라미터 서버에서 읽어오도록 수정했습니다. 이제 .launch 파일에서 로봇의 스펙을 쉽게 변경할 수 있습니다.
Frame ID 통일: child_frame_id를 base_footprint로 통일하여 일관성을 높였습니다.
초기 위치 설정 로직 개선: set_initial_2d 함수가 비표준적인 PoseStamped 메시지에 의존하지 않도록, 표준 Quaternion에서 Yaw 값을 올바르게 추출하는 로직으로 변경했습니다.

#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h> // ===== 추가됨 =====
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <cmath>

// Odometry data publishers
ros::Publisher odom_data_pub_quat;
nav_msgs::Odometry odomNew;
nav_msgs::Odometry odomOld;

// Robot physical constants
double WHEEL_RADIUS = 0.0535; // ===== 수정됨: 기본값 설정, 파라미터로 받을 예정 =====
double WHEEL_BASE = 0.360;  // ===== 수정됨: 기본값 설정, 파라미터로 받을 예정 =====
const double PI = 3.141592;

// Variables to store current wheel speeds and time
double left_wheel_speed = 0.0;  // m/s
double right_wheel_speed = 0.0; // m/s
ros::Time last_time;

// Get initial_2d message from Rviz clicks
void set_initial_2d(const geometry_msgs::PoseStamped &rvizClick) {
    // ===== 수정됨: Quaternion에서 Yaw 값을 올바르게 추출 =====
    tf2::Quaternion q;
    tf2::fromMsg(rvizClick.pose.orientation, q);
    tf2::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    odomOld.pose.pose.position.x = rvizClick.pose.position.x;
    odomOld.pose.pose.position.y = rvizClick.pose.position.y;
    odomOld.pose.pose.orientation.z = yaw; // Yaw 값을 저장
    ROS_INFO("Initial pose set to x: %f, y: %f, theta: %f", odomOld.pose.pose.position.x, odomOld.pose.pose.position.y, odomOld.pose.pose.orientation.z);
}

// Callback for left motor speed
void leftSpeedCallback(const std_msgs::Float32::ConstPtr& msg) {
    left_wheel_speed = msg->data;
}

// Callback for right motor speed
void rightSpeedCallback(const std_msgs::Float32::ConstPtr& msg) {
    right_wheel_speed = msg->data;
}

// Publish odometry message in quaternion format
void publish_quat_and_tf() { // ===== 수정됨: 함수 이름 변경 및 TF 발행 기능 추가 =====
    
    // ===== 추가됨: TF Broadcaster 생성 =====
    static tf2_ros::TransformBroadcaster br;
    geometry_msgs::TransformStamped transformStamped;

    tf2::Quaternion q;
    q.setRPY(0, 0, odomNew.pose.pose.orientation.z);

    // ===== 추가됨: TF 메시지 채우기 =====
    transformStamped.header.stamp = odomNew.header.stamp;
    transformStamped.header.frame_id = "odom";
    transformStamped.child_frame_id = "base_footprint";
    transformStamped.transform.translation.x = odomNew.pose.pose.position.x;
    transformStamped.transform.translation.y = odomNew.pose.pose.position.y;
    transformStamped.transform.translation.z = 0.0;
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();
    
    // TF 발행
    br.sendTransform(transformStamped);

    // Odometry 메시지 채우기 (Quaternion)
    nav_msgs::Odometry quatOdom;
    quatOdom.header.stamp = odomNew.header.stamp;
    quatOdom.header.frame_id = "odom";
    quatOdom.child_frame_id = "base_footprint"; // ===== 수정됨: base_footprint로 통일 =====
    quatOdom.pose.pose.position.x = odomNew.pose.pose.position.x;
    quatOdom.pose.pose.position.y = odomNew.pose.pose.position.y;
    quatOdom.pose.pose.position.z = 0.0;
    quatOdom.pose.pose.orientation.x = q.x();
    quatOdom.pose.pose.orientation.y = q.y();
    quatOdom.pose.pose.orientation.z = q.z();
    quatOdom.pose.pose.orientation.w = q.w();
    quatOdom.twist.twist.linear.x = odomNew.twist.twist.linear.x;
    quatOdom.twist.twist.angular.z = odomNew.twist.twist.angular.z;

    // ===== 수정됨: Covariance 버그 수정 (+= -> =) =====
    quatOdom.pose.covariance[0] = 0.01;
    quatOdom.pose.covariance[7] = 0.01;
    quatOdom.pose.covariance[14] = 1e6; // Z, Roll, Pitch는 사용 안하므로 매우 큰 값
    quatOdom.pose.covariance[21] = 1e6;
    quatOdom.pose.covariance[28] = 1e6;
    quatOdom.pose.covariance[35] = 0.1; // Yaw Covariance

    quatOdom.twist.covariance = quatOdom.pose.covariance; // Twist Covariance도 채워주는 것이 좋음
    
    odom_data_pub_quat.publish(quatOdom);
}

// Update odometry information
void update_odom() {
    ros::Time current_time = ros::Time::now();
    double dt = (current_time - last_time).toSec();
    last_time = current_time;

    // 로봇의 선형 및 각속도
    double v_x = (right_wheel_speed + left_wheel_speed) / 2.0;
    double v_th = (right_wheel_speed - left_wheel_speed) / WHEEL_BASE;

    // 위치 변화량 계산
    double delta_x = (v_x * cos(odomOld.pose.pose.orientation.z)) * dt;
    double delta_y = (v_x * sin(odomOld.pose.pose.orientation.z)) * dt;
    double delta_th = v_th * dt;

    // 새 위치 업데이트
    odomNew.pose.pose.position.x = odomOld.pose.pose.position.x + delta_x;
    odomNew.pose.pose.position.y = odomOld.pose.pose.position.y + delta_y;
    odomNew.pose.pose.orientation.z = odomOld.pose.pose.orientation.z + delta_th;

    // Theta 값을 -PI ~ PI 범위로 유지
    if (odomNew.pose.pose.orientation.z > PI) {
        odomNew.pose.pose.orientation.z -= 2 * PI;
    } else if (odomNew.pose.pose.orientation.z < -PI) {
        odomNew.pose.pose.orientation.z += 2 * PI;
    }

    // Odometry 메시지의 나머지 정보 채우기
    odomNew.header.stamp = current_time;
    odomNew.twist.twist.linear.x = v_x;
    odomNew.twist.twist.angular.z = v_th;

    // 다음 계산을 위해 현재 값을 old 값으로 저장
    odomOld = odomNew;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "odom_pub_speed");
    ros::NodeHandle node;
    ros::NodeHandle private_nh("~"); // ===== 추가됨: Private NodeHandle for parameters =====

    // ===== 추가됨: ROS 파라미터 서버에서 로봇 상수 값 불러오기 =====
    private_nh.param("wheel_radius", WHEEL_RADIUS, 0.0535);
    private_nh.param("wheel_base", WHEEL_BASE, 0.360);
    ROS_INFO("Wheel Radius: %f, Wheel Base: %f", WHEEL_RADIUS, WHEEL_BASE);

    // 초기값 설정
    odomOld.pose.pose.position.x = 0.0;
    odomOld.pose.pose.position.y = 0.0;
    odomOld.pose.pose.orientation.z = 0.0;
    odomNew = odomOld;

    // Subscriber & Publisher 설정
    ros::Subscriber subForRightSpeed = node.subscribe("right_motor_speed", 100, rightSpeedCallback);
    ros::Subscriber subForLeftSpeed = node.subscribe("left_motor_speed", 100, leftSpeedCallback);
    ros::Subscriber subInitialPose = node.subscribe("initial_2d", 1, set_initial_2d);
    odom_data_pub_quat = node.advertise<nav_msgs::Odometry>("odom_data_quat", 100);

    ros::Rate loop_rate(30);
    last_time = ros::Time::now();

    while(ros::ok()) {
        update_odom();
        publish_quat_and_tf(); // ===== 수정됨: 함수 이름 변경 =====
        ros::spinOnce();
        loop_rate.sleep();
    }
    // ===== 삭제됨: if(initialPoseRecieved) 조건문 삭제 =====
    return 0;
}