#include "senddata.h"


namespace uwb_slam
{
    void Senddata::Run(){

        ros::Rate loop_rate(100);
        position_pub_=nh_.advertise<nav_msgs::Odometry>("mapping",10);
        while(ros::ok()){
            publishOdometry();
            publishtf();
            ros::spinOnce();
            loop_rate.sleep();
        }


    }

    void Senddata::publishOdometry()
    {

        std::mutex mMutexSend;
        ros::Time current_time = ros::Time::now();

        // 设置 Odometry 消息的头部信息
        odom_imu_uwb_.header.stamp = current_time;
        odom_imu_uwb_.header.frame_id = "odom";  // 设置坐标系为 "map"
        odom_imu_uwb_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"

        // odom_imu_uwb_.header.frame_id = "odom";  
        // odom_imu_uwb_.child_frame_id = "map";  
        // 填充 Odometry 消息的位置信息
        odom_imu_uwb_.pose.pose.position.x = align_->uwb_->uwb_data_.x_;
        odom_imu_uwb_.pose.pose.position.y = align_->uwb_->uwb_data_.y_;
        odom_imu_uwb_.pose.pose.position.z = 0.0;
        

        // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
        odom_imu_uwb_.pose.pose.orientation.x = align_->qx;
        odom_imu_uwb_.pose.pose.orientation.y = align_->qy;
        odom_imu_uwb_.pose.pose.orientation.z = align_->qz;
        odom_imu_uwb_.pose.pose.orientation.w = align_->qw;


        // 发布 Odometry 消息
        position_pub_.publish(odom_imu_uwb_);


    }

    void Senddata::publishtf()
    {
        transformStamped.header.stamp = ros::Time::now();

        transformStamped.header.frame_id = "odom"; // 父坐标系为 "odom"
        transformStamped.child_frame_id = "base_link"; // 子坐标系为 "base_link"
        transformStamped.transform.translation.x = align_->syncPos.mat[0][0]/100; // 设置坐标变换的平移部分
        transformStamped.transform.translation.y = align_->syncPos.mat[1][0]/100;
        transformStamped.transform.translation.z = 0.0;
        transformStamped.transform.rotation.x = align_->qx; // 设置坐标变换的旋转部分（使用四元数表示）
        transformStamped.transform.rotation.y = align_->qy;
        transformStamped.transform.rotation.z = align_->qz;
        transformStamped.transform.rotation.w = align_->qw;

        broadcaster.sendTransform(transformStamped);
    }


    


} // namespace uwb_slam