agv_ws200731/src/imu_enc/src/imu_node.cpp

#include "imu_node.h"


ImuNode::ImuNode(std::string port, unsigned long baud){
    p_buffer = (char *)malloc((UARTBufferLength+2)*sizeof(char));
    p_temp_buf = (uint8_t *)malloc((iBufferSize+2)*sizeof(uint8_t));
    // p_temp_buffer = (char *)malloc(iBufferSize*sizeof(char));
    start = 0;
    end = 0;
    b_exit_ = false;
    read_thread_ = NULL;
    last_time_ = ros::Time::now();
    ori_time_ = ros::Time::now();
    last_pose_ = tf::Pose();
    last_vel_ = geometry_msgs::Twist();
    // port, baudrate, timeout in milliseconds
    try
    {
        my_serial = new serial::Serial(port, baud, serial::Timeout::simpleTimeout(1000));
        if (my_serial->isOpen())
        {
            read_thread_ = new boost::thread(boost::bind(ImuNode::thread_get_data, this));
        }
    }
    catch (serial::IOException &e)
    {
        std::cout << "serial exception: " << e.what() << std::endl;
        b_exit_ = true;
    }
    catch (std::exception &e)
    {
        std::cerr << "Unhandled Exception: " << e.what() << std::endl;
        b_exit_ = true;
    }
    // std::cout<<my_serial.isOpen()<<std::endl;
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom",2);
}

ImuNode::~ImuNode(){
    if (!ros::ok)
    {
        b_exit_ = true;
    }
    if (read_thread_!=NULL)
    {
        std::cout<<read_thread_<<" thread waiting to stop."<<std::endl;
        read_thread_->join();
    }
    free(p_buffer);
    free(p_temp_buf);
}

void ImuNode::publish_data(){
    if (!b_exit_ && b_connected_)
    {
        // usleep(10 * 1000);
        curr_time_ = ros::Time::now();
        double dt = (curr_time_ - last_time_).toSec();
        // std::vector<uint8_t> buff;
        // my_serial->read(buff, 100);
        // int start_index=0,end_index=100;
        std::unique_lock<std::mutex> lck(mtx);
        if (JY901.CopeSerialData(p_buffer, start, end, 5))
        // if (JY901.CopeSerialData((char *)buff.data(), start_index, end_index, 100))
        {
            // 赋值当前位置与速度
            tf::Vector3 last_pos = last_pose_.getOrigin();
            // 利用加速度计算速度
            // curr_vel_.linear.x = last_vel_.linear.x + dt * (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8;
            // curr_vel_.linear.y = last_vel_.linear.y + dt * (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8;
            // curr_vel_.linear.z = last_vel_.linear.z + dt * ((float)JY901.stcAcc.a[2] / 32768 * 16 - 1) * 9.8;
            // 保存原始加速度,用于测试
            curr_vel_.linear.x = (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8;
            curr_vel_.linear.y = (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8;
            curr_vel_.linear.z = ((float)JY901.stcAcc.a[2] / 32768 * 16 - 1) * 9.8;

            // 利用陀螺仪生成角速度
            // curr_vel_.angular.x = last_vel_.angular.x + (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0;
            // curr_vel_.angular.y = last_vel_.angular.y + (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0;
            // curr_vel_.angular.z = last_vel_.angular.z + (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0;
			curr_vel_.angular.x = (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0;
			curr_vel_.angular.y = (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0;
			curr_vel_.angular.z = (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0;
            // 利用磁场综合获取角度信息，扩展角度范围从[-pi, pi]到[-2pi, 2pi]
            double angle[3] = {0.0};
            for (size_t i = 0; i < 3; i++)
            {
                angle[i] = (float)JY901.stcAngle.Angle[i] / 32768 * M_PI;
                // if (JY901.stcAngle.Angle[i] * JY901.lastAngle.Angle[i] < 0)
                // {
                //     std::cout<<i<<" 反号"<<std::endl;
                //     if (JY901.stcAngle.Angle[i] / 32768 * M_PI < -M_PI_2)
                //     {
                //         std::cout<<"---"<<std::endl;
                //         m_angle_status_[i] = 1;
                //     }
                //     else if (JY901.stcAngle.Angle[i] / 32768 * M_PI > M_PI_2)
                //     {
                //         std::cout<<"+++"<<std::endl;
                //         m_angle_status_[i] = 2;
                //     }
                // }
                // if (m_angle_status_[i] == 1)
                // {
                //     if (JY901.stcAngle.Angle[i] > 0)
                //     {
                //         m_angle_status_[i] = 0;
                //     }
                //     else
                //     {
                //         angle[i] += M_PI * 2;
                //     }
                // }
                // if (m_angle_status_[i] == 2)
                // {
                //     if (JY901.stcAngle.Angle[i] < 0)
                //     {
                //         m_angle_status_[i] == 0;
                //     }
                //     else
                //     {
                //         angle[i] -= M_PI * 2;
                //     }
                // }
            }

            tf::Quaternion quat;quat.setRPY(angle[0],angle[1],angle[2]);
            curr_pose_.setRotation(quat);
            
            // curr_pose_.setRotation(tf::Quaternion((float)JY901.stcAngle.Angle[0] / 32768 * M_PI,
            //                                     (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, 
            //                                     (float)JY901.stcAngle.Angle[2] / 32768 * M_PI));
            // // 积分获得位置信息，存在累计误差
            // curr_pose_.setOrigin(tf::Vector3(last_pos.getX() + (last_vel_.linear.x+curr_vel_.linear.x)/2.0 * dt,
            //                                 last_pos.getY() + (last_vel_.linear.y+curr_vel_.linear.y)/2.0 * dt,
            //                                 last_pos.getZ() + (last_vel_.linear.z+curr_vel_.linear.z)/2.0 * dt));
            curr_pose_.setOrigin(tf::Vector3(1, 0, 0));

            last_vel_ = curr_vel_;
            last_pose_ = curr_pose_;

            //std::cout << "----------- Time: " << (ros::Time::now().toNSec()-ori_time_.toNSec())/1000000 << "-----------" << std::endl;
            printf("Acc: %.4f %.4f %.4f\n", (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[2] / 32768 * 16 * 9.8);
            printf("Gyro: %.4f %.4f %.4f\n", (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0);
            printf("linear: %.4f %.4f %.4f\n", curr_vel_.linear.x, curr_vel_.linear.y, curr_vel_.linear.z);

            //printf("Angle: %.4f %.4f %.4f\n", (float)JY901.stcAngle.Angle[0] / 32768 * M_PI, (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, (float)JY901.stcAngle.Angle[2] / 32768 * M_PI);
		    //printf("Ang speed: %.4f %.4f %.4f\n", curr_vel_.angular.x, curr_vel_.angular.y, curr_vel_.angular.z);
			// std::cout<<curr_pose_.getBasis().getRow(0)[0] <<" "<< curr_pose_.getBasis().getRow(0)[1] <<" "<< curr_pose_.getBasis().getRow(0)[2] <<std::endl;
            // std::cout<<curr_pose_.getBasis().getRow(1)[0] <<" "<< curr_pose_.getBasis().getRow(1)[1] <<" "<< curr_pose_.getBasis().getRow(1)[2] <<std::endl;
            // std::cout<<curr_pose_.getBasis().getRow(2)[0] <<" "<< curr_pose_.getBasis().getRow(2)[1] <<" "<< curr_pose_.getBasis().getRow(2)[2] <<std::endl;
            // printf("quaternion: %.4f %.4f %.4f %.4f\n", quat.x(),quat.y(),quat.z(),quat.w());
        }
        else
        {
            curr_vel_.linear = geometry_msgs::Vector3();
            curr_vel_.angular = geometry_msgs::Vector3();
            curr_pose_.setRotation(last_pose_.getRotation());
            curr_pose_.setOrigin(last_pose_.getOrigin());
            printf("failed\n");
        }
        // 发布里程计信息与tf
        nav_msgs::Odometry odom;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";
        odom.header.stamp = curr_time_;
        tf::poseTFToMsg(curr_pose_, odom.pose.pose);
        odom.twist.twist.linear = curr_vel_.linear;
        odom.twist.twist.angular = curr_vel_.angular;
        odom_pub_.publish(odom);

        tf::StampedTransform tf_msg(curr_pose_, ros::Time::now(), "odom", "base_link");
        tf_broadcaster_.sendTransform(tf_msg);

        // std::cout<<"-----------"<< ros::Time::now().toSec() <<"-----------"<<std::endl;
        // printf("Acc:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[2] / 32768 * 16 * 9.8);
        // printf("Gyro:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0);
        // printf("Angle:\t%.3f\t %.3f\t %.3f\r\n", (float)JY901.stcAngle.Angle[0] / 32768 * M_PI, (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, (float)JY901.stcAngle.Angle[2] / 32768 * M_PI);
        // printf("Mag:%d %d %d\r\n", JY901.stcMag.h[0], JY901.stcMag.h[1], JY901.stcMag.h[2]);
        // printf("DStatus:%d %d %d %d\r\n", JY901.stcDStatus.sDStatus[0], JY901.stcDStatus.sDStatus[1], JY901.stcDStatus.sDStatus[2], JY901.stcDStatus.sDStatus[3]);
        last_time_ = curr_time_;
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "imu_node");
    ImuNode imu("/dev/ttyUSB0", 38400);
    ros::Rate loop_rate(200);
    while (ros::ok())
    {
        if(imu.b_exit_)
            return 1;
        imu.publish_data();
        ros::spinOnce();
        loop_rate.sleep();
    }
    std::cout<<"leave"<<std::endl;
    imu.b_exit_ = true;
    return 1;
}