ROS学习系列笔记(一)：Topic发布自定义msg数据及其rqt_plot可视化显示

代码包下载链接：https://github.com/yongzcong/communication_learn.git
内容：假设发布的数据包括三个值$x,y,z$，首先定义msg数据格式；其次新建发布节点publisher，发布msg格式的数据；然后新建节点listener，订阅发布消息；最后，在rqt_plot中订阅发布的数据并可视化显示。

1、msg格式数据与编译文件

1.1 定义数据结构

进入工作空间：
/catkin_ws/src/note/
在note/msg建立文件类型文件test.msg
写入消息的类型：

float64 x
float64 y
float64 z

注意：msg的文件名，后面的cpp文件均需包含此文件。

1.2 CMakeLists.txt文件

cmake_minimum_required(VERSION 2.8.3)
project(note)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  geometry_msgs
)

add_message_files(FILES test.msg)
generate_messages(DEPENDENCIES std_msgs)


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

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker ${PROJECT_NAME}_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener ${PROJECT_NAME}_generate_messages_cpp)

1.3 package.xml文件

<?xml version="1.0"?>
<package format="2">
  <name>note</name>
  <version>0.0.0</version>
  <description>The note package</description>
  <maintainer email="hcx@todo.todo">hcx</maintainer>

  <license>TODO</license>
  
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>
 

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>  
  
  <export>

  </export>
</package>

2. 定义发布节点

2.1 创建发布节点

进入工作空间：
/catkin_ws/src/note/
在note/src建立发布cpp文件talker.cpp
给出代码：

/**
 * 该例程将发布self_date话题，消息类型float64
*/
#include "ros/ros.h"
#include <math.h>
#include "note/test.h"

int main(int argc, char **argv)
{
  // ROS节点初始化
  ros::init(argc, argv, "shiyan_talker");
  
  // 创建节点句柄
  ros::NodeHandle n;
  
  // 创建一个Publisher，发布名为self_date的topic，消息类型为std_msgs::float32
  ros::Publisher self_pub = n.advertise<note::test>("self_date", 1000);

  int pi = 3.1415926;

  // 设置循环的频率
  ros::Rate loop_rate(10);

  int count = 0;
  while (ros::ok())
  {
    // 初始化std_msgs::float32类型的消息
    note::test self_msg;
    self_msg.x = sin(pi*count/16);
    self_msg.y = cos(pi*count/16);
    self_msg.z = 1 + (pow(-1,count)/2);

    // 发布消息
    ROS_INFO("%.5f %.5f %.5f",self_msg.x,self_msg.y,self_msg.z);
    self_pub.publish(self_msg);
	// 循环等待回调函数
    ros::spinOnce();
    
	// 按照循环频率延时
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

2.2 发布节点程序解释

下面详细解释代码：

1. 头文件

#include "ros/ros.h"
#include <math.h>
#include "note/test.h"

此为头文件，ros/ros.h是ros的头文件；math.h是数学功能头文件，因为后面用到三角函数；note/test.h是数据格式包含。
2. ros节点初始化与创建节点句柄

  ros::init(argc, argv, "shiyan_talker");
  ros::NodeHandle n;

3. 创建发布

ros::Publisher self_pub = n.advertise<note::test>("self_date", 1000);

创建一个Publisher，发布名为self_date的topic，消息类型为float64。其中，self_pub是名称，note::test指定数据类型；self_date是话题名；1000是发布序列的大小，这个影响不大，取1000就行了。
注意话题名，订阅消息需要指定发布者节点名。
4. 数据赋值

note::test self_msg;
self_msg.x = sin(pi*count/16);
self_msg.x = sin(pi*count/16);
self_msg.z = 1 + (pow(-1,count)/2);

self_msg是定义的test类型的数据结构，其中有float64 x,float64 y,float64 z,分别赋值给定。
5. 消息发布

ROS_INFO("%.5f %.5f %.5f",self_msg.x,self_msg.y,self_msg.z);
self_pub.publish(self_msg);

通过self_pub将test类型的数据结构self_msg发布出去。

2.3 发布节点运行结果

3、如何订阅自己的msg数据

3.1 定义发布节点

进入工作空间：
/catkin_ws/src/note/
在note/src建立订阅cpp文件listener.cpp
给出代码：

#include "ros/ros.h"
#include "note/test.h"

void chatterCallback(const note::test::ConstPtr& msg)
{
    ROS_INFO("%f %f %f",msg->x,msg->y,msg->z);
}

int main(int argc, char **argv)
{
 
    ros::init(argc, argv, "shiyan_listener");
 
    ros::NodeHandle n;
 
    ros::Subscriber sub = n.subscribe("self_date", 1000, chatterCallback);

    ros::spin();

    return 0;
}

3.2 发布节点程序解释

下面详细解释代码：

1. 头文件

#include "ros/ros.h"
#include "note/test.h"

此为头文件，ros/ros.h是ros的头文件；note/test.h是数据格式包含。
2. ros节点初始化与创建节点句柄

  ros::init(argc, argv, "shiyan_listener");
  ros::NodeHandle n;

3. 创建订阅

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

创建一个Subscriber，订阅名为sub。其中，self_date是订阅的话题名（与发布的话题名对应），chatterCallback是回调函数；1000是接收序列的大小，这个影响不大，与发布者保持一致就好。
4. 回调函数

void chatterCallback(const note::test::ConstPtr& msg)
{
    ROS_INFO("%f %f %f",msg->x,msg->y,msg->z);
}

chatterCallback是回调函数名，note::test::ConstPtr指定msg的数据类型，然后将订阅的消息打印出来。

3.3 发布节点运行结果

4、如何可视化自己发布的msg数据

新打开终端，首先对工作空间source，否则会出错，找不到话题。
source ~/catkin_ws2/devel/setup.bash
rosrun rqt_plot rqt_plot
输入话题名self_date即可将订阅的数据通过曲线表示出来。
注：点与点之间通过直线连接，因此self_date.c会以近似三角波的形式呈现，self_date.c本身是上下震荡的点。