ROS初学——消息发布、消息接收与armadillo矩阵库的使用

ROS初学——消息发布、消息接收与armadillo矩阵库的使用

ROS初学记录，消息的发布和接收，代码文件编写、cmakelist文件编写，package.xml文件编写，armadillo矩阵库的使用。

消息发布

编写了一个简单的消息发布节点，发布的数据类型为std_msgs中定义的数据类型。节点名称为test_sensor_talker，发送的消息名称为sensor001，是一个Float32类型的数据。std_msgs中定义了很多基础的数据类型，可以在ROS网站上查到，注意，使用不同的数据类型时需要修改头文件。

//test_sensor_talker.cpp消息发布节点
#include<sstream>
#include"ros/ros.h"
#include"std_msgs/Float32.h"

int main(int argc,char **argv)
{
    ros::init(argc,argv,"test_sensor_talker");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::Float32>("sensor001",1000);
    ros::Rate loop_rate(10);
    int count =0;
    while(ros::ok())
    {
        std_msgs::Float32 msg;
        float ss=(float)count;
        msg.data=ss;
 	ROS_INFO("%0.2f",msg.data);
         chatter_pub.publish(msg);
         ros::spinOnce();
         loop_rate.sleep();
         count++;
	//count=count%10;
    }
    return 0;
}

对应的CmakeList.txt

cmake_minimum_required(VERSION 2.8.3)
project(test_sensor)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS
  message_runtime
#  INCLUDE_DIRS include
#  LIBRARIES test_sensor
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
   roscpp std_msgs
)


include_directories(
  include ${catkin_INCLUDE_DIRS}
# include
  ${catkin_INCLUDE_DIRS}
)

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

对应的package.xml中添加

<buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>

消息订阅与自定义消息类型

消息订阅并使用类处理

完成消息订阅节点后，接下来完成消息订阅处理节点，并且消息处理的节点希望在类中完成，所以消息订阅节点定义了两个文件，一个是test_sensor_KF.cpp用来订阅消息，另一个是FusionCenter_KF.h，类写在FusionCenter_KF.h中，该文件放在include文件夹中。

//test_sensor_KF.cpp
#include"ros/ros.h"
#include"FusionCenter_KF.h"

int main(int argc,char **argv)
{
    ros::init(argc, argv, "test_sensor_KF");
    ros::NodeHandle n;
    FusionCenter_KF kf;
    ros::Subscriber sub = n.subscribe("sensor001",1000,&FusionCenter_KF::chatterCallback,&kf);
    ros::Rate loop_rate(10);
   while(ros::ok())
    {
         ros::spinOnce();
         kf.Print_z();
         loop_rate.sleep();
    }
    return 0;
}

//FusionCenter_KF.h
#include<iostream>
#include"std_msgs/Float32.h"
#include"test_sensor/sensor2d.h"
using namespace std;
struct meas_re
{
    float time;
    float r;
    float theta;
    float pos_x;
    float pos_y;
};

class FusionCenter_KF
{
    public:
        test_sensor::sensor2d  z;
        void Print_z();
        void chatterCallback(const test_sensor::sensor2d::ConstPtr& msg);
};

void FusionCenter_KF::Print_z()
{
        cout<<z.time<<endl;
        cout<<z.pos_x<<endl;
        cout<<z.pos_y<<endl;
        cout<<z.r<<endl;
        cout<<z.theta<<endl;
}
void FusionCenter_KF::chatterCallback(const test_sensor::sensor2d::ConstPtr& msg)
{
        z.time=msg->time;
        z.pos_x=msg->pos_x;
        z.pos_y=msg->pos_y;
        z.r=msg->r;
        z.theta=msg->theta;
}

test_sensor_KF.cpp中对于类的引用直接使用#include"FusionCenter_KF.h"即可，如果将FusionCenter_KF.h放在include中test_sensor文件夹中，则需要改成#include"test_sesor/FusionCenter_KF.h"。
不使用类处理时，

ros::Subscriber sub = n.subscribe("chatter",1000,chatterCallback);

其中chatterCallback为回调函数，在主函数外定义，使用类处理时，参数变成了4个。

 ros::Subscriber sub = n.subscribe("sensor001",1000,&FusionCenter_KF::chatterCallback,&kf);

ros spin的两种方式

while(ros::ok())
    {
         ros::spinOnce();//运行一次，后面的代码继续运行
         kf.Print_z();
         loop_rate.sleep();
    }
   与
   ros::spin()//该行后的代码不运行

定义消息

消息文件定义在msg文件夹中，文件名为消息名称.msg，使用时和结构体用法类似，该文件在编译后会生成同名的.h文件，引用格式为：

#include"test_sensor/sensor2d.h"//格式：项目名/消息名.h

CMakeList.txt写法：在findpackage中加入message_generation，增加add_message_files片段，在其中添加msg文件名，添加generate_messages片段，在其中添加自定义消息的依赖，还需要添加target_link_libraries(test_sensor_talker armadillo)

cmake_minimum_required(VERSION 2.8.3)
project(test_sensor)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  message_generation
)

## Generate messages in the 'msg' folder
 add_message_files(
   FILES
   sensor2d.msg
 )

## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   std_msgs
 )

## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
  CATKIN_DEPENDS
  message_runtime
#  INCLUDE_DIRS include
#  LIBRARIES test_sensor
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
   roscpp std_msgs
)

include_directories(
  include 
	${catkin_INCLUDE_DIRS}
# include
  #${catkin_INCLUDE_DIRS}
)

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(test_sensor_talker src/test_sensor_talker.cpp)
target_link_libraries(test_sensor_talker ${catkin_LIBRARIES})
target_link_libraries(test_sensor_talker armadillo)
add_executable(test_sensor_KF src/test_sensor_KF.cpp)
target_link_libraries(test_sensor_KF ${catkin_LIBRARIES})

package.xml中需要添加：

<build_depend>message_generation</build_depend>

ROS中使用armadillo库

库文件在armadillo官网下载，然后就行编译安装，即可在ROS节点代码中使用。
参考链接：ubuntu 安装 armadillo

tar -Jxvf armadillo-7.200.2.tar.xz
cd armadillo-7.200.2
cmake .
make
sudo make install

调用方法：

#include<armadillo>
using namespace arma