ROS使用GY25T IMU发布IMU_data Topic

gy25t 是一款低成本,具有加速度计和陀螺仪计二合一的模块,模块将二者数据融合输出IMU数据,数据包括欧拉角,加速度、陀螺角速度。数据接口支持串口和IIC。针对ROS中需要的IMU数据,现在分享一下ros中串口接收gy25t数据并发布topic。

首先先用上位机配置好模块的波特率为115200,加速度量程+-4G,陀螺量程+-2000,输出频率100hz,航向角阀值0.25度/s,陀螺自校准开启,最后保存配置。这些配置保存后,以后模块上电就无需再配置。之后的串口读取或者IIC读取数据就方便了。

第二步编写程序,工程依赖roscpp、rospy、std_msgs、tf,编写的程序主要是配置串口和对接收到的数据的解析。

//serial_port.cpp
#include <ros/ros.h>
#include <boost/asio.hpp> //包含boost库函数
#include <boost/bind.hpp>
#include <iostream>
#include <string>
#include <cstring>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>

#include <std_msgs/Float32.h>
using namespace std;
using namespace boost::asio;

unsigned char buf[200],raw_buffer[200],gy25t_id=0xa4;

uint8_t gy_count=0,reg_index=0,reg_number=0,start_reg=0;

struct gy_sensor
{
int16_tacc[3];
int16_tgyro[3];
int16_trpy[3];
};
gy_sensor gy25t;
uint8_t check_sum(unsigned char *buff,uint8_t len)
{
uint8_tsum=0,i=0;
while(len–)
{
sum+=buff[i++];
}
returnsum;
}
 
uint8_t read_datas_analysis(unsigned char *read_buffer,uint16_t len)
{
uint16_ti=0;

while(len!=i)
{
switch(gy_count)
{
case0:
if(read_buffer[i]==gy25t_id)
{
raw_buffer[0]=read_buffer[i];
gy_count++;
} else
gy_count=0;
break;
case1:
if(read_buffer[i]==0x03)
{
raw_buffer[1]=read_buffer[i];
gy_count++;
} else
gy_count=0;
break;
case2:
if(read_buffer[i]<0x23)
{
start_reg=raw_buffer[2]=read_buffer[i];
gy_count++;
} else
gy_count=0;
break;
case3:
if(read_buffer[i]<=0x23)
{
reg_number=raw_buffer[3]=read_buffer[i];
reg_index= start_reg+reg_number;
if(reg_index<=0x23)
gy_count++;
else
gy_count=0;
} else
gy_count=0;
break;
default:
raw_buffer[gy_count]=read_buffer[i];
gy_count++;
if(reg_number==(gy_count-5))
{
uint8_tsum=check_sum(raw_buffer,(gy_count-1));
if(sum==raw_buffer[gy_count-1])
{
 
gy25t.acc[0]=raw_buffer[4]<<8|raw_buffer[5];
gy25t.acc[1]=raw_buffer[6]<<8|raw_buffer[7];
gy25t.acc[2]=raw_buffer[8]<<8|raw_buffer[9];

gy25t.gyro[0]=raw_buffer[10]<<8|raw_buffer[11];
gy25t.gyro[1]=raw_buffer[12]<<8|raw_buffer[13];
gy25t.gyro[2]=raw_buffer[14]<<8|raw_buffer[15];

gy25t.rpy[0]=raw_buffer[16]<<8|raw_buffer[17];
gy25t.rpy[1]=raw_buffer[18]<<8|raw_buffer[19];
gy25t.rpy[2]=raw_buffer[20]<<8|raw_buffer[21];

gy_count=0;
reg_number=0;
returntrue;
}
else
{
 
// cout <<“faild” << endl;


}
reg_number=0;
gy_count=0;
}
// cout <<“gycount:”<<dec<<(int)gy_count<< endl;
break;

}
 
i++;
 
}
returnfalse;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, “serial_test1”);
//创建句柄
ros::NodeHandlen;
ros::PublisherIMU_pub = n.advertise<sensor_msgs::Imu>(“IMU_data”, 20);
ros::Raterate(100);
//创建timeout
io_serviceiosev;
serial_portsp(iosev, “/dev/ttyUSB0”); //定义 cout << my_gy.gyro[2] << endl; 传输的串口
sp.set_option(serial_port::baud_rate(115200));
sp.set_option(serial_port::flow_control());
sp.set_option(serial_port::parity());
sp.set_option(serial_port::stop_bits());
sp.set_option(serial_port::character_size(8));
unsignedcharsendbuf[5]={0xa4,0x03,0x08,18,0};//set out datas type
unsignedcharsendbuf1[5]={0xa4,0x06,0x06,0x10,0};//acc/gyro cal
unsignedcharsendbuf2[5]={0xa4,0x06,0x06,0x01,0};//set yaw=0

sendbuf[4]=check_sum(sendbuf,4);
sp.write_some(buffer(sendbuf));
ros::Duration(1).sleep();
sendbuf1[4]=check_sum(sendbuf1,4);
sp.write_some(buffer(sendbuf1));
ros::Duration(1).sleep();
sendbuf2[4]=check_sum(sendbuf2,4);
sp.write_some(buffer(sendbuf2));
int16_tcount_loop=0;
unsignedlongsys_time=0,last_time=0;
while (ros::ok())
{
 
size_tread_len;

read_len=sp.read_some(buffer(buf));
if(read_datas_analysis(buf,read_len))
{
ROS_INFO(“accxyz:[%d,%d,%d,]”,gy25t.acc[0],gy25t.acc[1],gy25t.acc[2]);
ROS_INFO(“gyroxyz:[%d,%d,%d,]”,gy25t.gyro[0],gy25t.gyro[1],gy25t.gyro[2]);

floatroll=((float)gy25t.rpy[0])/100.0f,pitch=((float)gy25t.rpy[1])/100.0f,yaw=((float)gy25t.rpy[2])/100.0f;
ROS_INFO(“rpy:[%.2f,%.2f,%.2f,]”,roll,pitch,yaw);
 
 
sensor_msgs::Imuimu_data;
imu_data.header.stamp=ros::Time::now();
imu_data.header.frame_id=”base_link”;
 
imu_data.linear_acceleration.x = (((double)gy25t.acc[0])/8192*9.8);
imu_data.linear_acceleration.y = (((double)gy25t.acc[1])/8192*9.8);
imu_data.linear_acceleration.z = 0;
imu_data.angular_velocity.x=(((double)gy25t.gyro[0])/16.4f)/57.3;
imu_data.angular_velocity.x=(((double)gy25t.gyro[1])/16.4f)/57.3;
imu_data.angular_velocity.x=(((double)gy25t.gyro[2])/16.4f)/57.3;
 
tf::Quaternionqtn;
qtn.setRPY(roll/57.3,pitch/57.3,yaw/57.3);
imu_data.orientation.w=qtn.getW();
imu_data.orientation.x=qtn.getX();
imu_data.orientation.y=qtn.getY();
imu_data.orientation.z=qtn.getZ();
sys_time= imu_data.header.stamp.sec;
 

IMU_pub.publish(imu_data);
}

 
ros::spinOnce();
rate.sleep();
 
}
 
iosev.run();
return0;


}

编译程序无误后,将模块用USB转TTL模块连接好,程序中有对串口号的选择serial_portsp(iosev, “/dev/ttyUSB0”);/dev/ttyUSB0是对应我模块使用的串口号,大家使用的时候注意根据自己的来更改,对于查看串口号,我是用arduino来查看的。最后模块接入电脑,运行该topic,打开rviz,我使用的是rviz_imu_plugin插件来显示IMU效果。

程序中最开始发送的三个指令

unsigned char sendbuf[5]={0xa4,0x03,0x08,18,0};//set out datas type
unsigned char sendbuf1[5]={0xa4,0x06,0x06,0x10,0};//acc/gyro cal
unsigned char sendbuf2[5]={0xa4,0x06,0x06,0x01,0};//set yaw=0
sendbuf是配置模块输出的数据有哪些。sendbuf1是校准加计,该指令可使模块roll、pitch归零,在使用该指令时模块必须是静止状态。sendbuf2是让模块的yaw角归零。
注意:模块默认输出的加速度为机体坐标系下的重力加速度和运动加速度之和,对于世界坐标系下的线速度可通过坐标系转换得到。

《ROS使用GY25T IMU发布IMU_data Topic》有1条评论

发表评论