ROS高效入门第三章 -- 自定义ROS消息，编写C++ pub+sub样例，理解计算图源名称

1 资料

关于ros消息和话题的原理，参考本人的ROS高效入门博客第二章的2.5节：ROS高效入门第二章 – 基本概念和常用命令学习，基于小乌龟样例
本文是把下面两个参考资料相应的例子重写了一下，并组织成自己的目录。新手可以跟着博客，很丝滑地学会如何写ROS消息和话题的C++程序，并理解其中的原理。读者也可以直接挑选感兴趣的样例进行研究。
（1）《机器人操作系统（ROS）浅析》[美] Jason M. O’Kane 著 肖军浩 译，第3，5和6章
（2）ros Tutorials 初级教程的10~12节： ros Tutorials

2 正文

2.1 第一个ros程序，hello_ros

（1）创建hello_ros软件包

cd ~/catkin_ws/src
// hello_ros依赖std_msgs，rospy和roscpp
catkin_create_pkg hello_ros std_msgs rospy roscpp
// 创建hello.cpp和launch文件
cd hello_ros
touch src/hello.cpp
mkdir launch
touch launch/start.launch

文件树

（2）编写hello.cpp

// ros标志头文件，ros程序的标配
#include 

int main(int argc, char** argv) {
	// 初始化ros节点，最后一个参考是节点名
    ros::init(argc, argv, "hello_ros");
    // 创建ros节点句柄，其会把节点注册到master中
    ros::NodeHandle nh;
	
	// info等级打印hello
    ROS_INFO("hello ros");
    return 0;
}

（3）编写CmakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(hello_ros)

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs)

// 申明catkin package
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})

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

（4）编写launch文件

<launch>

launch>

（5）编译

cd ~/catkin_ws
catkin_make --source src/hello_ros/

（6）运行
命令行方式：

cd ~/catkin_ws/
source devel/setup.bash
roscore   // 一个窗口
rosrun hello_ros hello_ros_node			//另一个窗口

launch文件启动方式：

cd ~/catkin_ws/
source devel/setup.bash
roslaunch hello_ros start.launch

2.2 最简单的pub+sub样例，收发一个string

（1）创建simple_pub_sub软件包，并创建文件

cd ~/catkin_ws/src
catkin_create_pkg simple_pub_sub std_msgs rospy roscpp

cd simple_pub_sub/
touch src/pub.cpp
touch src/sub.cpp
mkdir launch
touch launch/start.launch

（2）编写pub.cpp

#include 
#include 

int main(int argc, char **argv) {
    ros::init(argc, argv, "sim_pub");
    ros::NodeHandle nh;
	// 建立pub句柄，topic为/chatter，消息类型为std_msgs/String，队列长度为1000
    ros::Publisher std_pub = nh.advertise<std_msgs::String>("chatter", 1000);
    // 设置10hz发送频率
    ros::Rate loop_rate(10);
    int count = 0;
    while (ros::ok()) {
        std_msgs::String msg;
        msg.data = "hello ycao " + std::to_string(count++);
        ROS_INFO("%s", msg.data.c_str());

        std_pub.publish(msg);
		
		// 这个接口是让节点检查并调用回调函数（这里没有回调函数，但一般都带着它）
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

（3）编写sub.cpp

#include 
#include 

void std_cb(const std_msgs::String::ConstPtr &msg) {
    ROS_INFO("i received: %s", msg->data.c_str());
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "sim_sub");
    ros::NodeHandle nh;
	// 建立sub句柄，订阅/chatter topic，队列长度是1000，回调函数是std_cb
    ros::Subscriber std_sub = nh.subscribe("chatter", 1000, &std_cb);
	// 这个函数相当于一个循环，内部调用spinOnce，即时刻检查并调用回调函数。
    ros::spin();
    return 0;
}

（4）编写CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(simple_pub_sub)
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs)
catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(sim_pub src/pub.cpp)
add_executable(sim_sub src/sub.cpp)
target_link_libraries(sim_pub ${catkin_LIBRARIES})
target_link_libraries(sim_sub ${catkin_LIBRARIES})

（5）编写start.launch

>

>

>

>

（6）编译并运行

cd ~/catkin_ws
catkin_make --source  src/simple_pub_sub/
source devel/setup.bash
roslaunch simple_pub_sub start.launc

2.3 自定义msg，写pub+sub测试

（1）创建msf_self软件包，并创建文件

cd ~/catkin_ws/src
catkin_create_pkg msf_self std_msgs rospy roscpp

cd msf_self/
touch src/pub.cpp
touch src/sub.cpp
mkdir launch
touch launch/start.launch
mkdir msg
touch msg/Student.msg

（2）编写Student.msg

string name
uint8 age

（3）编写pub.cpp

#include 
// 引入自定义msg头文件，位于devel的include内
#include 

int main(int argc, char **argv) {
    ros::init(argc, argv, "msg_pub");
    ros::NodeHandle nh;
    // topic是/student，消息类型为msg_self::Student
    ros::Publisher msg_pub = nh.advertise<msg_self::Student>("student", 1000);

    ros::Rate loop_rate(10);
    while (ros::ok()) {
        msg_self::Student msg;
        msg.name = "jieshoudaxue";
        msg.age = 30;
        ROS_INFO("name = %s, age = %u", msg.name.c_str(), msg.age);

        msg_pub.publish(msg);

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

（4）编写sub.cpp

#include 
#include 

void stu_cb(const msg_self::Student::ConstPtr &msg) {
    ROS_INFO("i received: %s, %u", msg->name.c_str(), msg->age);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "msg_sub");
    ros::NodeHandle nh;

    ros::Subscriber num_sub = nh.subscribe("student", 1000, &stu_cb);

    ros::spin();
    return 0;
}

（5）编写CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(msg_self)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation	// 自定义消息，这个必须加
)

add_message_files(
  FILES
  Student.msg
)

generate_messages(
  DEPENDENCIES
  std_msgs		// 自定义消息里，有string和uint8，这些在std_msgs里
)

catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs message_runtime)

include_directories(${catkin_INCLUDE_DIRS})

add_executable(${PROJECT_NAME}_pub src/pub.cpp)
add_executable(${PROJECT_NAME}_sub src/sub.cpp)
// 这个依赖必须加，不然cpp内找不到msg_self/Student.h
add_dependencies(${PROJECT_NAME}_pub msg_self_generate_messages_cpp)
add_dependencies(${PROJECT_NAME}_sub msg_self_generate_messages_cpp)

target_link_libraries(${PROJECT_NAME}_pub ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_sub ${catkin_LIBRARIES})

（6）修改package.xml，添加

  <build_depend>message_generationbuild_depend>
  <exec_depend>message_runtimeexec_depend>

（7）编写start.launch

<launch>

<node
    pkg="msg_self"
    type="msg_self_pub"
    name="msg_self_pub"
    required="true"
    output="screen"
/>

<node
    pkg="msg_self"
    type="msg_self_sub"
    name="msg_self_sub"
    respawn="true"
    output="screen"
/>

launch>

（8）编译和运行

cd ~/catkin_ws
catkin_make --source src/msg_self/
source devel/setup.bash
roslaunch msg_self start.launch

2.4 基于turtlesim，写一个复杂点的pub+sub

（1）创建handle_turtlesim软件包，其依赖turtlesim，内部有一个pub节点，用来向turtlesim发送随机运动命令，另一个sub节点，订阅turtlesim发出来的位置信息，并打印出来。

cd ~/catkin_ws/src
// 依赖turtlesim 和geometry_msgs 
catkin_create_pkg handle_turtlesim turtlesim geometry_msgs rospy roscpp

cd handle_turtlesim/
touch src/pub.cpp
touch src/sub.cpp
mkdir launch
touch launch/start.launch

（2）编写pub.cpp

#include 
#include 
#include 

int main(int argc, char** argv) {
    ros::init(argc, argv, "pub_velocity");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);

    srand(time(0));
    ros::Rate loop_rate(2);
    while(ros::ok()) {
        geometry_msgs::Twist msg;
        msg.linear.x = double(rand())/double(RAND_MAX);
        msg.angular.z = double(rand())/double(RAND_MAX);
        ROS_INFO("sending rand velocity cmd: linear = %f, angular = %f", msg.linear.x, msg.angular.z);

        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

（3）编写sub.cpp

#include 
#include 

void processMsg(const turtlesim::Pose& msg) {
    ROS_INFO("position: [%f, %f], direction: %f", msg.x, msg.y, msg.theta);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "sub_pose");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("turtle1/pose", 1000, &processMsg);

    ros::spin();
    return 0;
}

（4）编写CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(handle_turtlesim)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  turtlesim
)
catkin_package(CATKIN_DEPENDS geometry_msgs roscpp rospy turtlesim)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(${PROJECT_NAME}_pub src/pub.cpp)
add_executable(${PROJECT_NAME}_sub src/sub.cpp)
target_link_libraries(${PROJECT_NAME}_pub
  ${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_sub
  ${catkin_LIBRARIES}
)

（5）编写start.launch

<launch>
// 指定命名空间，这样可以起很多个同名节点，如果不指定，就是默认命名空间“/”
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    respawn="true"
    ns="sim1"		
/>
<node
    pkg="handle_turtlesim"
    type="handle_turtlesim_pub"
    name="handle_turtlesim_pub"
    required="true"
    output="screen"
    ns="sim1"
/>
<node
    pkg="handle_turtlesim"
    type="handle_turtlesim_sub"
    name="handle_turtlesim_sub"
    output="screen"
    ns="sim1"
/>

launch>

（6）编译并运行

cd ~/catkin_ws
catkin_make --source src/handle_turtlesim/
source devel/setup.bash
roslaunch handle_turtlesim start.launch

2.5 计算图源名称（graph resource name）

（1）节点、话题、服务和参数统称为计算图源，均用一个字符串标识。以2.4节的handle_turtlesim为例，主要涉及节点和话题，节点名是ros::init()函数指定的，话题名是创建pub和sub句柄时指定的，如下。


（2）这里需要引入一个概念，即“全局名称”和“相对名称”。全局名称指以“/”开头的名称，无论用作参数，还是写在cpp里，都没有任何二义性。而上面截图里的，都是相对名称，程序运行时需要解析为全局名称，也就是把两个名称拼在一块。
仍以handle_turtlesim举例，如果handle_turtlesim没有指定命名空间，则默认命名空间就是“/”：


如果handle_turtlesim指定命名空间，例如start.launch里的：ns=“sim1”，则默认命名空间就是/sim1，运行时的全局名称，就会带着/sim1。

ros提供相对名称这套机制，最大的好处是为package的移植提供便利，用户能方便地将某个节点和话题移植到其他的命名空间，而不用担心命名冲突。
（3）除了上面所说的“全局名称”和“相对名称”，还有一个“私有名称”，以波浪号打头“~” ，如：~max_vel。
ros运行时，也需要将私有名称解析为全局名称。与相对名称不同的是，私有名称不是用当前默认命名空间，而是用的它们节点名称作为命名空间。如节点名为：/turtlesim1，私有名称为：~max_vel，则相应的全局名称为：/turtlesim1/max_vel。
通常情况下，如果一个节点内的计算图源，只在该节点存在，则可以使用私有名称。

3 总结

本文中的例子放在了本人的github上： ros_src