维特智能IMU 接入ROS发布IMU数据类型话题

最新推荐文章于 2024-08-31 03:32:19 发布

ganjb1

最新推荐文章于 2024-08-31 03:32:19 发布

阅读量3.3k

点赞数 10

分类专栏： ros 文章标签： c++

本文链接：https://blog.csdn.net/ganjb1/article/details/118153554

版权

本文介绍了如何将维特智能的HWT901B IMU接入ROS系统，通过串口读取数据并发布为ROS话题。首先，详细讲述了准备工作，包括安装串口功能包、创建功能包以及查看串口状态。然后，说明了如何修改官方驱动以读取IMU数据并发布话题。最后，展示了在rviz中配置和显示IMU数据的方法，需要确保已安装imu-tools功能包。

摘要由CSDN通过智能技术生成

#维特智能IMU 接入ROS发布IMU数据类型话题

1.准备工作

1.1安装串口功能包

sudo apt-get install ros-melodic-serial

1.2创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg robot_imu std_msgs rospy roscpp serial tf

在~/catkin_ws/src/robot_imu/src/创建imu_com.cpp,编辑程序
在CMakeList.txt中添加

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(imu_com src/imu_com.cpp src/JY901.cpp)
target_link_libraries(imu_com ${catkin_LIBRARIES})

我使用的是维特智能的HWT901B的IMU
这里要用到他官方C++驱动程序的JY901.cpp和JY901.h文件，后文会给出。

1.3查看串口挂载情况

ls -l /dev/ttyUSB*

如果只有一个USB设备的话，默认情况下是USB0，需要打开串口权限。注意这种方法只是临时的，并不是永久的。

sudo chmod 777 /dev/ttyUSB0

2.读取IMU数据并发布话题

编辑imu_com.cpp文件

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <serial/serial.h> 
#include <robot_imu/JY901.h>
# include <sensor_msgs/Imu.h>
#include <sstream>
#include "tf/transform_datatypes.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);   //发送串口数据 
} 
extern char sum;
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_raw", 1000);
    //打开串口
    try 
    {
    
    //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(9600); 
        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; 
    } 
    unsigned short  data_size;
    unsigned char   tmpdata[2000] ;
    float           roll,pitch,yaw;
    //消息发布频率
	ros::Rate loop_rate(100);
    while (ros::ok()){
   
		//处理从串口来的Imu数据
		//串口缓存字符数
        if(data_size = ser.available()){
    //ser.available(当串口没有缓存时，这个函数会一直等到有缓存才返回字符数
            ser.read(tmpdata, data_size);
            JY901.CopeSerialData( tmpdata,data_size);   //JY901 imu 库函数
           //打包IMU数据
            sensor_msgs::Imu imu_data;
            
            imu_data.header.stamp = ros::Time::