ROS 学习篇(六)发布imu和gps消息

版权声明:本文为博主原创文章,遵循 CC 4.0 by-sa 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/Tansir94/article/details/81385812

一、创建工作空间

mkdir -p imu_gps/src  #创建工作空间
catkin_make  #编译
source devel/setup.bash  #将新的工作空间加入到环境变量中
echo $ROS_PACKAGE_PATH   #输入如下指令查看环境变量

二、创建IMU_data发布程序包

首先下方链接地址中是sensor_msgs/Imu.msg的消息格式:
http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html

cd imu_gps/src #进入工作空间中的src

#创建一个名为'imu_gps_publish'的新程序包,这个程序包依赖于std_msgs、roscpp和rospy:
catkin_create_pkg imu_gps_publish stdmsgs roscpp rospy  

此时整个工作空间的文件架构如下:

imu_gps   #工作空间
        build
        devle
        src
            imu_gps_publish
                include
                src
                CMakeLists.txt
                package.xml
      CMakeLists.txt

在imu_gps_publish文件夹下的src文件中新建imu_gps_data.cpp

cd imu_gps_publish/src 
touch imu_gps_data.cpp

将如下代码复制到新建的cpp文件中

// Step 1:  Include Library Headers:
//发布imu的数据到IMU_data
#include <ros/ros.h> 
#include <sensor_msgs/Imu.h>

int main(int argc, char** argv)
{
    // Step 2: Initialization:
       ros::init(argc, argv, "imu_gps");     
    ros::NodeHandle n;  

    ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("IMU_data", 20);  
    ros::Rate loop_rate(50);  
    while(ros::ok())
    {

            sensor_msgs::Imu imu_data;
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            //四元数位姿,所有数据设为固定值,可以自己写代码获取IMU的数据,,然后进行传递
           imu_data.orientation.x = 0;
            imu_data.orientation.y = -1;
            imu_data.orientation.z = -5;
            imu_data.orientation.w = 6;
            //线加速度
            imu_data.linear_acceleration.x = 0.01; 
            imu_data.linear_acceleration.y = 0.02;
            imu_data.linear_acceleration.z = 0.03;
        //角速度
            imu_data.angular_velocity.x = 0.05; 
            imu_data.angular_velocity.y = 0.06; 
            imu_data.angular_velocity.z = 0.07;



            IMU_pub.publish(imu_data);



        ros::spinOnce();  
        loop_rate.sleep();  
    }

    return 0;
}

更改imu_gps_publish下的CMakeLists.txt文件,添加如下两行

add_executable(${PROJECT_NAME}_node src/imu_gps_data.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
 )

返回工作空间编译:

cd
cd imu_gps
catkin_make 

编译成功,运行结果如下:
这里写图片描述
现在尝试运行节点:打开roscore,运行节点程序

roscore
rosrun imu_gps_publish imu_gps_publish_node

在新终端中查看节点和话题以及发布的消息如下图:
这里写图片描述

注:若是在CMakeList.txt中添加的两行为:

add_executable(imu_gps src/imu_gps_data.cpp)
target_link_libraries(imu_gps ${catkin_LIBRARIES})

重新编译运行时,启动节点的命令为

rosrun imu_gps_publish imu_gps 

可见,add_executable意思为添加可执行的文件,imu_gps或是${PROJECT_NAME}_node指的是运行的节点的名称。

三、在IMU_data发布的基础上加入GPS数据发布

GPS消息类型文档:
http://docs.ros.org/jade/api/sensor_msgs/html/msg/NavSatFix.html
在imu_gps_data.cpp文件中添加gps数据消息的发布,最终代码:

// Step 1:  Include Library Headers:
//发布imu的数据到IMU_data
#include <ros/ros.h> 
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

int main(int argc, char** argv)
{
    // Step 2: Initialization:
    ros::init(argc, argv, "imu_gps");     
    ros::NodeHandle n;  
    //定义IMU数据的消息发布
    ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("IMU_data", 20);  
    //定义GPS数据的消息发布
    ros::Publisher GPS_pub = n.advertise<sensor_msgs::NavSatFix>("GPS_data", 20);

    ros::Rate loop_rate(10);  
    while(ros::ok())
    {
//imu消息的赋值和发布                   
            sensor_msgs::Imu imu_data;
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            //四元数位姿,所有数据设为固定值,可以自己写代码获取IMU的数据,,然后进行传递
            imu_data.orientation.x = 0;
            imu_data.orientation.y = -1;
            imu_data.orientation.z = -5;
            imu_data.orientation.w = 6;
            //线加速度
            imu_data.linear_acceleration.x = 0.01; 
            imu_data.linear_acceleration.y = 0.02;
            imu_data.linear_acceleration.z = 0.03;
        //角速度
            imu_data.angular_velocity.x = 0.05; 
            imu_data.angular_velocity.y = 0.06; 
            imu_data.angular_velocity.z = 0.07;
            //发布IMU
            IMU_pub.publish(imu_data);


//gps消息的赋值和发布
            sensor_msgs::NavSatFix gps_data;
            gps_data.header.stamp = ros::Time::now();
            gps_data.header.frame_id = "base_link";

            //gps_data.status="working";
            gps_data.latitude=36.6555;
            gps_data.longitude=117.2656;
            gps_data.altitude=40.5;

            GPS_pub.publish(gps_data);

        ros::spinOnce();  
        loop_rate.sleep();  
    }

    return 0;
}

添加工作空间环境变量,运行节点:

source devel/setup.bash
rosrun imu_gps_publish imu_gps

查看节点和消息:
查看消息和节点,以及GPS_data节点发布的消息
这里写图片描述

最终,实现了在一个节点中的IMU和GPS的数据发布。

展开阅读全文

没有更多推荐了,返回首页