陀螺仪

Posted 2021-03-18 serser

#include "ros/ros.h"

#include "std_msgs/String.h"

#include <serial/serial.h>

#include <sensor_msgs/Imu.h>

#include <sstream>

#include <tf/tf.h>

serial::Serial ser; //声明串口对象

//回调函数

void write_callback(const std_msgs::String::ConstPtr& msg)

{

    ROS_INFO_STREAM("Writing to serial port" <<msg->data);

    ser.write(msg->data);   //发送串口数据

}

int imuClear(void)

{

     unsigned char angleClearCmd[5]={0x68,0x04,0x00,0x28,0x2C};

     unsigned char accClearCmd[5]={0x68,0x04,0x00,0x27,0x2B};

     ser.write(angleClearCmd,5);

     ros::Duration(0.5).sleep();

     ser.write(accClearCmd,5);

     ros::Duration(0.5).sleep();

}

int main(int argc, char **argv)

{

    //初始化节点

    ros::init(argc, argv, "serial_imu_node");

    //声明节点句柄

    ros::NodeHandle nh;

    //订阅主题，并配置回调函数

    ros::Subscriber IMU_write_pub = nh.subscribe("imu_command", 1000, write_callback);

    //发布主题, 消息格式使用sensor_msg::Imu标准格式（topic名称，队列长度）

    ros::Publisher IMU_read_pub = nh.advertise<sensor_msgs::Imu>("/imu/data", 1000);

    //打开串口

    try

    {

    //设置串口属性，并打开串口

        ser.setPort("/dev/ttyUSB0");

        ser.setBaudrate(115200);

        serial::Timeout to = serial::Timeout::simpleTimeout(1000);

        ser.setTimeout(to);

        ser.open();

    }

    catch (serial::IOException& e)

    {

        ROS_ERROR_STREAM("Unable to open port ");

        return -1;

    }

    //检测串口是否已经打开，并给出提示信息

    if(ser.isOpen())

    {

        ROS_INFO_STREAM("Serial Port initialized");

    }

    else

    {

        return -1;

    }

    imuClear();

    //消息发布频率

    ros::Rate loop_rate(100);

    while (ros::ok()){

        //处理从串口来的Imu数据

	//串口缓存字符数

	unsigned char  data_size,i;

        if(data_size = ser.available()){ //ser.available(当串口没有缓存时，这个函数会一直等到有缓存才返回字符数

           unsigned char  tmpdata[data_size] ;

           ser.read(tmpdata, data_size);

           unsigned char sign,byte1,byte2,byte3,byte4,byte5;

           float roll,pitch,yaw,Gyro_X,Gyro_Y,Gyro_Z,Acc_X,Acc_Y,Acc_Z;

           unsigned char crc=0;

           for(i=0;i<data_size;i++)

           {

                if((i!=0) && (i!=(data_size-1)))crc+=tmpdata[i];

		ROS_INFO("tmpdata[%d]:%02x",i,tmpdata[i]);

           }

           if((tmpdata[0]!=0x68)||(tmpdata[data_size-1]!=crc))continue;

           if(tmpdata[4]&0x10)roll=(float)(-1*(0.01*(tmpdata[6]&0x0f)+0.1*((tmpdata[6]&0xf0)>>4)+(tmpdata[5]&0x0f)+10*((tmpdata[5]&0xf0)>>4)+100*(tmpdata[4]&0x0f)));

           else roll=(float)(0.01*(tmpdata[6]&0x0f)+0.1*((tmpdata[6]&0xf0)>>4)+(tmpdata[5]&0x0f)+10*((tmpdata[5]&0xf0)>>4)+100*(tmpdata[4]&0x0f));

           if(tmpdata[7]&0x10)pitch=(float)(-1*(0.01*(tmpdata[9]&0x0f)+0.1*((tmpdata[9]&0xf0)>>4)+(tmpdata[8]&0x0f)+10*((tmpdata[8]&0xf0)>>4)+100*(tmpdata[7]&0x0f)));

           else pitch=(float)(0.01*(tmpdata[9]&0x0f)+0.1*((tmpdata[9]&0xf0)>>4)+(tmpdata[8]&0x0f)+10*((tmpdata[8]&0xf0)>>4)+100*(tmpdata[7]&0x0f));

           if(tmpdata[10]&0x10)yaw=(float)(-1*(0.01*(tmpdata[12]&0x0f)+0.1*((tmpdata[12]&0xf0)>>4)+(tmpdata[11]&0x0f)+10*((tmpdata[11]&0xf0)>>4)+100*(tmpdata[10]&0x0f)));

           else yaw=(float)(0.01*(tmpdata[12]&0x0f)+0.1*((tmpdata[12]&0xf0)>>4)+(tmpdata[11]&0x0f)+10*((tmpdata[11]&0xf0)>>4)+100*(tmpdata[10]&0x0f));

           roll = (float)((roll/180.0)*3.14);

           pitch = (float)((pitch/180.0)*3.14);

           yaw = (float)((yaw/180.0)*3.14);

           if(tmpdata[13]&0x10)Acc_X=(float)(-1*(0.001*(tmpdata[15]&0x0f)+0.01*((tmpdata[15]&0xf0)>>4)+0.1*(tmpdata[14]&0x0f)+((tmpdata[14]&0xf0)>>4)+10*(tmpdata[13]&0x0f)));

           else Acc_X=(float)(0.001*(tmpdata[15]&0x0f)+0.01*((tmpdata[15]&0xf0)>>4)+0.1*(tmpdata[14]&0x0f)+((tmpdata[14]&0xf0)>>4)+10*(tmpdata[13]&0x0f));

           if(tmpdata[16]&0x10)Acc_Y=(float)(-1*(0.001*(tmpdata[18]&0x0f)+0.01*((tmpdata[18]&0xf0)>>4)+0.1*(tmpdata[17]&0x0f)+((tmpdata[17]&0xf0)>>4)+10*(tmpdata[16]&0x0f)));

           else Acc_Y=(float)(0.001*(tmpdata[18]&0x0f)+0.01*((tmpdata[18]&0xf0)>>4)+0.1*(tmpdata[17]&0x0f)+((tmpdata[17]&0xf0)>>4)+10*(tmpdata[16]&0x0f));

           if(tmpdata[19]&0x10)Acc_Z=(float)(-1*(0.001*(tmpdata[21]&0x0f)+0.01*((tmpdata[21]&0xf0)>>4)+0.1*(tmpdata[20]&0x0f)+((tmpdata[20]&0xf0)>>4)+10*(tmpdata[19]&0x0f)));

           else Acc_Z=(float)(0.001*(tmpdata[21]&0x0f)+0.01*((tmpdata[21]&0xf0)>>4)+0.1*(tmpdata[20]&0x0f)+((tmpdata[20]&0xf0)>>4)+10*(tmpdata[19]&0x0f));

           if(tmpdata[22]&0x10)Gyro_X=(float)(-1*(0.01*(tmpdata[24]&0x0f)+0.1*((tmpdata[24]&0xf0)>>4)+(tmpdata[23]&0x0f)+10*((tmpdata[23]&0xf0)>>4)+100*(tmpdata[22]&0x0f)));

           else Gyro_X=(float)(0.01*(tmpdata[24]&0x0f)+0.1*((tmpdata[24]&0xf0)>>4)+(tmpdata[23]&0x0f)+10*((tmpdata[23]&0xf0)>>4)+100*(tmpdata[22]&0x0f));

           if(tmpdata[25]&0x10)Gyro_Y=(float)(-1*(0.01*(tmpdata[27]&0x0f)+0.1*((tmpdata[27]&0xf0)>>4)+(tmpdata[26]&0x0f)+10*((tmpdata[26]&0xf0)>>4)+100*(tmpdata[25]&0x0f)));

           else Gyro_Y=(float)(0.01*(tmpdata[27]&0x0f)+0.1*((tmpdata[27]&0xf0)>>4)+(tmpdata[26]&0x0f)+10*((tmpdata[26]&0xf0)>>4)+100*(tmpdata[25]&0x0f));

           if(tmpdata[28]&0x10)Gyro_Z=(float)(-1*(0.01*(tmpdata[30]&0x0f)+0.1*((tmpdata[30]&0xf0)>>4)+(tmpdata[29]&0x0f)+10*((tmpdata[29]&0xf0)>>4)+100*(tmpdata[28]&0x0f)));

           else Gyro_Z=(float)(0.01*(tmpdata[30]&0x0f)+0.1*((tmpdata[30]&0xf0)>>4)+(tmpdata[29]&0x0f)+10*((tmpdata[29]&0xf0)>>4)+100*(tmpdata[28]&0x0f));

           Gyro_X = (float)((Gyro_X/180.0)*3.14);

           Gyro_Y = (float)((Gyro_Y/180.0)*3.14);

           Gyro_Z = (float)((Gyro_Z/180.0)*3.14);

           //打包IMU数据

           Acc_X = (float)(Acc_X*9.786283718603544);

           Acc_Y = (float)(Acc_Y*9.786283718603544);

           Acc_Z = (float)(Acc_Z*9.786283718603544);

           ROS_INFO("roll:%3.3f,pitch:%3.3f,raw:%3.3f",roll,pitch,yaw);

           ROS_INFO("Acc_X:%3.3f,Acc_Y:%3.3f,Acc_Z:%3.3f",Acc_X,Acc_Y,Acc_Z);

           ROS_INFO("Gyro_X:%3.3f,Gyro_Y:%3.3f,Gyro_Z:%3.3f",Gyro_X,Gyro_Y,Gyro_Z);

           tf::Quaternion quaternion_ = tf::createQuaternionFromRPY(roll, pitch, yaw);

           sensor_msgs::Imu imu_data;

           imu_data.header.stamp = ros::Time::now();

           imu_data.header.frame_id = "/imu";

           imu_data.orientation.x = quaternion_.x();

           imu_data.orientation.y = quaternion_.y();

           imu_data.orientation.z = quaternion_.z();

           imu_data.orientation.w = quaternion_.w();

           ROS_INFO("imu_data.orientation.x:%3.3f,imu_data.orientation.y:%3.3f,imu_data.orientation.z:%3.3f,imu_data.orientation.w:%3.3f",imu_data.orientation.x,imu_data.orientation.y,imu_data.orientation.z,imu_data.orientation.w);

           //tf::quaternionMsgToTF(imu_data.orientation, quaternion_);

           imu_data.angular_velocity.x = Gyro_X;

           imu_data.angular_velocity.y = Gyro_Y;

           imu_data.angular_velocity.z = Gyro_Z;

           imu_data.linear_acceleration.x = Acc_X;

           imu_data.linear_acceleration.y = Acc_Y;

           imu_data.linear_acceleration.z = Acc_Z;

           //发布topic

           IMU_read_pub.publish(imu_data);

        }

        //处理ROS的信息，比如订阅消息,并调用回调函数

        ros::spinOnce();

        loop_rate.sleep();

  }

  	return 0;

 }