以WTGAHRS2为例。
代码为测试版,数据位置和转换不一定正确(比如经纬只能输出整数,四元数放在了GPS数据位置)。由于本人时间条件,来不及修改,请急需的自行修改。
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "ImuGpsRaw/IMU_Data.h"
#include "ImuGpsRaw/GPS_Data.h"
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
using namespace std;
using namespace Eigen;
Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q = rollAngle *yawAngle *pitchAngle;
return q;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "Integrated_Navigation");
ros::NodeHandle n;
//创建imu数据话题发布
ros::Publisher Pub_IMU_Data = n.advertise<ImuGpsRaw::IMU_Data>("Wit/IMU",100);
ros::Publisher Pub_GPS_Data = n.advertise<ImuGpsRaw::GPS_Data>("Wit/GPS",100);
//创建一个serial对象
serial::Serial sp;
//创建timeout
serial::Timeout to = serial::Timeout::simpleTimeout(100);
//设置要打开的串口名称
sp.setPort("/dev/ttyUSB0");
//设置串口通信的波特率
sp.setBaudrate(9600);
//串口设置timeout
sp.setTimeout(to);
double ax,ay,az;
double wx,wy,wz;
double angle_roll,angle_pitch,angle_yaw;
double longitude, latitude, altitude;
double GPS_Yaw, GPSV;
double q0, q1, q2, q3;
int satellite;
double PDOP, HDOP, VDOP;
Eigen::Quaterniond q;
ImuGpsRaw::IMU_Data imu_data;
ImuGpsRaw::GPS_Data GPS_data;
try
{
//打开串口
sp.open();
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
//判断串口是否打开成功
if(sp.isOpen())
{
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
}
else
{
return -1;
}
ros::Rate loop_rate(100);
while(ros::ok())
{
//获取缓冲区内的字节数
size_t n = sp.available();
if(n!=0)
{
uint8_t buffer[1024];
//读出数据
n = sp.read(buffer, n);
for(int i=0; i<n; i++)
{
if(buffer[i]==0X55) //IMU地址
{
if(buffer[i+1]==0x51) //加速度帧
{
signed short int temp;
temp=buffer[i+3];
temp=temp<<8;
temp=(temp|buffer[i+2]);
ax=(temp/32768.0)*16*9.8051;
temp=buffer[i+5]; //加速度八位
temp=temp<<8;
temp=(temp|buffer[i+4]); //加速度高八位
ay=(temp/32768.0)*16*9.8051;
temp=buffer[i+7]; //加速度低八位
temp=temp<<8;
temp=(temp|buffer[i+6]); //加速度高八位
az=(temp/32768.0)*16*9.8051;
// std::cout<<"ax:"<<ax<<std::endl;
// std::cout<<"ay:"<<ay<<std::endl;
// std::cout<<"az:"<<az<<std::endl;
i+=11;
}
if(buffer[i+1]==0x52) //角速度帧
{
signed short int temp;
temp=buffer[i+3];
temp=temp<<8;
temp=(temp|buffer[i+2]);
wx=(temp/32768.0)*2000;
temp=buffer[i+5]; //角速度低八位
temp=temp<<8;
temp=(temp|buffer[i+4]); //角速度高八位
wy=(temp/32768.0)*2000;
temp=buffer[i+7]; //角速度低八位
temp=temp<<8;
temp=(temp|buffer[i+6]); //角速度高八位
wz=(temp/32768.0)*2000;
// std::cout<<"wx:"<<wx<<std::endl;
// std::cout<<"wy:"<<wy<<std::endl;
// std::cout<<"wz:"<<wz<<std::endl;
i+=11;
}
if(buffer[i+1]==0x53) //角度帧
{
unsigned short int temp;
temp=buffer[i+3]; //角度低八位
temp=temp<<8;
temp=(temp|buffer[i+2]); //角度高八位
angle_roll=(temp/32768.0)*180;
temp=buffer[i+5]; //角度低八位
temp=temp<<8;
temp=(temp|buffer[i+4]); //角度高八位
angle_pitch=(temp/32768.0)*180;
temp=buffer[i+7]; //角度低八位
temp=temp<<8;
temp=(temp|buffer[i+6]); //角度高八位
angle_yaw=(temp/32768.0)*180;
q = euler2Quaternion(angle_roll, angle_pitch, angle_yaw) ;
// std::cout<<"angle_roll:"<<angle_roll<<std::endl;
// std::cout<<"angle_pitch:"<<angle_pitch<<std::endl;
// std::cout<<"angle_yaw:"<<angle_yaw<<std::endl;
i+=11;
}
if(buffer[i+1]==0x57) //GPS帧
{
unsigned long int temp1;
unsigned long int temp2;
unsigned long int temp3;
unsigned long int temp4;
temp1=buffer[i+3]; //
temp1=temp1<<8;
temp2=buffer[i+4]; //
temp2=temp2<<16;
temp3=buffer[i+5]; //
temp3=temp3<<24;
temp4=(temp3|temp2|temp1|buffer[i+2]); //
longitude = int(temp4 /10000000)*100 + (temp4 % 10000000) / 100000;
temp1=buffer[i+7]; //
temp1=temp1<<8;
temp2=buffer[i+8]; //
temp2=temp2<<16;
temp3=buffer[i+9]; //
temp3=temp3<<24;
temp4=(temp3|temp2|temp1|buffer[i+6]); //
latitude = int(temp4 /10000000)*100 + (temp4 % 10000000) / 100000;
// std::cout<<"longitude :"<<longitude <<std::endl;
// std::cout<<"latitude :"<<latitude <<std::endl;
i+=11;
}
if(buffer[i+1]==0x58) //GPS帧
{
unsigned short int temp;
unsigned long int temp1;
unsigned long int temp2;
unsigned long int temp3;
unsigned long int temp4;
temp=buffer[i+3]; //
temp=temp<<8;
temp=(temp|buffer[i+2]); //
altitude = temp / 10;
temp=buffer[i+5]; //
temp=temp<<8;
temp=(temp|buffer[i+4]); //
GPS_Yaw = temp / 100;
temp1=buffer[i+7]; //
temp1=temp1<<8;
temp2=buffer[i+8]; //
temp2=temp2<<16;
temp3=buffer[i+9]; //
temp3=temp3<<24;
temp4=(temp3|temp2|temp1|buffer[i+6]); //
GPSV = temp4 / 1000;
// std::cout<<"altitude:"<<altitude<<std::endl;
// std::cout<<"GPS_Yaw:"<<GPS_Yaw<<std::endl;
// std::cout<<"GPSV:"<<GPSV<<std::endl;
i+=11;
}
if(buffer[i+1]==0x59) //四元数
{
signed short int temp;
temp=buffer[i+3];
temp=temp<<8;
temp=(temp|buffer[i+2]);
q0 = temp / 32768;
temp=buffer[i+5];
temp=temp<<8;
temp=(temp|buffer[i+4]);
q1 = temp / 32768;
temp=buffer[i+7];
temp=temp<<8;
temp=(temp|buffer[i+6]);
q2 = temp / 32768;
temp=buffer[i+9];
temp=temp<<8;
temp=(temp|buffer[i+8]);
q3 = temp / 32768;
i+=11;
}
if(buffer[i+1]==0x5A) //GPS精度
{
signed short int temp;
temp=buffer[i+3];
temp=temp<<8;
temp=(temp|buffer[i+2]);
satellite = temp;
temp=buffer[i+5];
temp=temp<<8;
temp=(temp|buffer[i+4]);
PDOP = temp / 100;
temp=buffer[i+7];
temp=temp<<8;
temp=(temp|buffer[i+6]);
HDOP = temp / 100;
temp=buffer[i+9];
temp=temp<<8;
temp=(temp|buffer[i+8]);
VDOP = temp / 100;
i+=11;
}
imu_data.IMU_Data.header.stamp = ros::Time::now();
imu_data.IMU_Data.header.frame_id = "base_link";
imu_data.IMU_Data.linear_acceleration.x = ax;
imu_data.IMU_Data.linear_acceleration.y = ay;
imu_data.IMU_Data.linear_acceleration.z = az;
imu_data.IMU_Data.angular_velocity.x = wx;
imu_data.IMU_Data.angular_velocity.y = wy;
imu_data.IMU_Data.angular_velocity.z = wz;
imu_data.IMU_Data.orientation.x = q.x();
imu_data.IMU_Data.orientation.y = q.y();
imu_data.IMU_Data.orientation.z = q.z();
imu_data.IMU_Data.orientation.w = q.w();
imu_data.Roll = angle_roll;
imu_data.Pitch = angle_pitch;
imu_data.Yaw = angle_yaw;
Pub_IMU_Data.publish(imu_data);
GPS_data.GPS_Data.header.stamp =ros::Time::now();
GPS_data.GPS_Data.header.frame_id = "base_link";
GPS_data.GPS_Data.latitude = latitude;
GPS_data.GPS_Data.longitude = longitude;
GPS_data.GPS_Data.altitude = altitude;
GPS_data.DOPP = PDOP;
GPS_data.DOPH = HDOP;
GPS_data.DOPV = VDOP;
GPS_data.satellite = satellite;
GPS_data.GPS_Yaw = GPS_Yaw;
GPS_data.q0 = q0;
GPS_data.q1 = q1;
GPS_data.q2 = q2;
GPS_data.q3 = q3;
Pub_GPS_Data.publish(GPS_data);
}
}
// std::cout << std::endl;
//sp.write(buffer, n);
}
loop_rate.sleep();
}
//关闭串口
sp.close();
return 0;
}
代码上传在github里
GitHub - casso1993/wit_ros: This is the ROS code for wit sensor