ROS 学习篇（六）发布imu和gps消息

一、创建工作空间

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_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_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

查看节点和消息：

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