【ROS_Kinetic】订阅 / 发布节点 | launch 文件编写 | 自定义消息类型的订阅与发布

1. 发布 / 订阅节点 ( C++实现 )

1.1 发布 ( talker.cpp)

#include "ros/ros.h"
#include "std_msgs/String.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    ros::Rate loop_rate(10);
    int count = 0;

    while (ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world" << count;
        msg.data = ss.str();
        ROS_INFO("%s", msg.data.c_str());
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        count++;
    }
}

1.2 订阅 ( listener.cpp)

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr &msg)
{
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
    ros::spin();
    return 0;
}

2. 发布订阅节点 ( Python2 实现 )

2.1 发布 ( talker.py)

#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from std_msgs.msg import String


def talker():
    pub = rospy.Publisher("chatter", String, queue_size=10)
    rospy.init_node("talker", anonymous=False)
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInternalException:
        pass

# 需要将该 python 文件的属性设置为可执行文件
# bash 命令: rosrun practice1 talker.py

2.2 订阅 ( listener.py)

#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from std_msgs.msg import String


def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)


def listener():
    rospy.init_node("listener", anonymous=False)
    rospy.Subscriber("chatter", String, callback)
    rospy.spin()


if __name__ == '__main__':
    listener()

3. 编写 launch 文件

3.0 小 tip

使用 launch 文件就不需要 roscore来启动 ROS 了。

3.1 针对 C++ 节点

• talker.launch 内容如下：

<launch>
    <node pkg="practice1" type="talker" name="talker" output="screen"/>
</launch>

3.2 针对 Python2 节点

• listener.launch 内容如下：

<launch>
    <node pkg="practice1" type="listener.py" name="listener" output="screen"/>
</launch>

3.3 同时启动多个节点

• 启动 talker.cpp 节点和 listener.py 节点：

<launch>
    <node pkg="practice1" type="talker" name="talker"/>
    <node pkg="practice1" type="listener.py" name="listener" output="screen"/>
</launch>

3.4 同时启动多个 launch 文件

<launch>
    <include file="$(find practice1)/launch/talker.launch"/>
    <include file="$(find practice1)/launch/listener.launch"/>
</launch>

4. 自定义消息类型并编写发布订阅 ( Python2 实现)

4.1 自定义消息

1. 在 practice下新建msg目录，创建Student.msg文件。
   
2. 在上述文件中写入如下内容：

   string first_name
   string last_name
   uint8 age
   uint32 score
   

3. 修改CMakeList.txt文件

   find_package(catkin REQUIRED
       roscpp
       std_msgs
       message_generation  # define message type by myself
   )
   

   add_message_files(
   FILES
   Student.msg
   # Message1.msg
   # Message2.msg
   )
   

   generate_messages(
     DEPENDENCIES
     std_msgs  # Or other packages containing msgs
   )
   

   catkin_package(
   #  INCLUDE_DIRS include
   #  LIBRARIES practice1
   #  CATKIN_DEPENDS other_catkin_pkg
   CATKIN_DEPENDS message_runtime
   #  DEPENDS system_lib
   )
   

4. 修改package.xml文件：

     <build_depend>message_runtime</build_depend>
     <build_export_depend>message_runtime</build_export_depend>
   

5. 指定编译当前功能包：

   ~/welcomeRobot_ws$ catkin_make -DCATKIN_WHITELIST_PACKAGES=practice1
   

   该命令生成了 C++ 和 Python2 的依赖项：
   
6. 验证消息是否生成：

   ~$ rosmsg show practice1/Student
   

   输出如下：
   

4.2 发布消息 ( msg_pub.py)

#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from practice1.msg import Student  # /工作空间根目录/devel/lib/python2.7/dist-packages/practice1/msg/_Student.py


def msg_pub():
    pub = rospy.Publisher('student_info', Student, queue_size=10)
    rospy.init_node('msg_pub', anonymous=False)
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        student = Student()
        student.first_name = "ZhangSan"
        student.last_name = "Robot"
        student.age = 2
        student.score = 100
        rospy.loginfo('pub a student info.')  # 打印日志
        pub.publish(student)
        rate.sleep()


if __name__ == '__main__':
    try:
        msg_pub()
    except rospy.ROSInternalException:
        pass

到根目录下执行如下命令运行并查看话题：

roscore # 新终端
rosrun practice1 msg_pub.py # 新终端
rostopic echo /student_info # 新终端

4.3 订阅消息 ( msg_sub.py)

#!/usr/bin/env python
# -*- coding:utf-8 -*-

import rospy
from practice1.msg import Student  # /工作空间根目录/devel/lib/python2.7/dist-packages/practice1/msg/_Student.py


def callable(data):
    rospy.loginfo("Student Name is %s %s; Age is %d; Score is %d.",
                  data.first_name, data.last_name, data.age, data.score)


def msg_sub():
    rospy.init_node('msg_sub', anonymous=False)
    rospy.Subscriber('student_info', Student, callable)
    rospy.spin()


if __name__ == '__main__':
    try:
        msg_sub()
    except rospy.ROSInternalException:
        pass

5. 自定义消息类型并编写发布订阅 ( C++ 实现 )

5.0 修改 /xxx_ws/.vscode/c_cpp_properties.json文件

添加自定义消息的所在路径：

"includePath": [
	"/home/用户名/xxx_ws/devel/include/**" // 该目录的绝对路径
]

5.1 发布消息 ( msg_pub.cpp)

#include "ros/ros.h"
#include "practice1/Student.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "msg_pub");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<practice1::Student>("student_info", 1000);
    ros::Rate loop_rate(10);
    uint8_t count = 0;
    while (ros::ok())
    {
        practice1::Student student;
        student.first_name = "ZhangSan";
        student.last_name = "Robot";
        student.age = count;
        student.score = 100;
        chatter_pub.publish(student);

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

在 src/practice1/CMakeLists.txt中添加：

add_executable(msg_pub src/msg_pub.cpp)
target_link_libraries(msg_pub ${catkin_LIBRARIES})
add_dependencies(msg_pub practice1_generate_messages_cpp) # 让这句话先执行以先去生成消息文件

5.2 订阅消息 ( msg_sub.cpp)

#include "ros/ros.h"
#include "practice1/Student.h"

void callback(const practice1::Student::ConstPtr &msg)
{
    ROS_INFO("Student's name is %s %s; Age is %d; Score is %d", msg->first_name.c_str(), msg->last_name.c_str(), msg->age, msg->score);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "msg_sub");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("student_info", 1000, callback);
    ros::spin();

    return 0;
}

在 src/practice1/CMakeLists.txt中添加：

add_executable(msg_sub src/msg_sub.cpp)
target_link_libraries(msg_sub ${catkin_LIBRARIES})
add_dependencies(msg_sub practice1_generate_messages_cpp) # 让这句话先执行以先去生成消息文件

6. 自定义服务类型并编写服务端客户端节点 ( Python2 实现 )

6.1 自定义服务类型

新建 src/practice1/srv/AddTwoints.srv文件，写入如下内容：

int64 a
int64 b
--- # 分隔符上面是请求，下面是响应
int64 sum

在 src/practice1/CMakeLists.txt中修改：

add_service_files(
  FILES
  AddTwoints.srv
  # Service1.srv
  # Service2.srv
)

编译工作空间：

catkin_make

新终端中验证：

rossrv show practice1/AddTwoints

输出如下：

在 .vscode/settings.json文件中添加一条路径以方便在编写python代码时可以显示提示和自动补全：

"/home/用户名/xxx_ws/devel/lib/python2.7/dist-packages"

6.2 服务端 ( add_two_ints_server.py)

#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
from practice1.srv import AddTwoints, AddTwointsResponse


def handle_add_two_inits(req):
    print("Returning [%s + %s = %s]" % (req.a, req.b, (req.a + req.b)))
    return AddTwointsResponse(req.a + req.b)


def add_two_ints_server():
    rospy.init_node('add_two_ints_server')
    s = rospy.Service('add_two_ints', AddTwoints,
                      handle_add_two_inits)  # 回调函数取名为 handle_add_two_inits
    print("Ready to add two ints.")
    rospy.spin()


if __name__ == '__main__':
    add_two_ints_server()

在终端执行：

roscore # 新终端
rosrun practice1 add_two_ints_server.py # 新终端
rossrv list # 新终端，查看所有服务类型
rosservice list # 查看正在运行的服务类型
rosservice call /add_two_ints 3 5 # 调用服务，计算 3 + 5

输出结果：

xxx@xxxxxx:~$ rosservice call /add_two_ints 3 5
sum: 8

6.3 客户端 ( add_two_ints_client.py)

#!/usr/bin/env python
# -*- coding:utf-8 -*-
import sys
import rospy
from practice1.srv import *


def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')  # 阻塞等待
    try:
        add_two_ints = rospy.ServiceProxy(
            'add_two_ints',
            AddTwoints)  # Create a handle to a ROS service for invoking calls.
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)


if __name__ == '__main__':
    if len(sys.argv) == 3:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
    else:
        sys.exit(1)

    print("Requesting %s + %s" % (x, y))
    print("%s + %s = %s" % (x, y, add_two_ints_client(x, y)))

运行：

roscore
rosrun practice1 add_two_ints_server.py
rosrun practice1 add_two_ints_client.py 1 3

7. 自定义服务类型并编写服务端客户端 ( C++ 实现 )

7.1 服务端 ( add_two_ints_server.cpp)

#include "ros/ros.h"
#include "practice1/AddTwoints.h"

bool add(practice1::AddTwoints::Request &req, practice1::AddTwoints::Response &res)
{
    res.sum = req.a + req.b;
    ROS_INFO("request: x = %ld, y = %ld", (long int)req.a, (long int)req.b);
    ROS_INFO("sending back response: [%ld]", (long int)res.sum);
    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "add_two_ints_server"); // 初始化节点
    ros::NodeHandle n;

    ros::ServiceServer service = n.advertiseService("add_two_ints", add);
    ROS_INFO("Ready to add two ints.");
    ros::spin();

    return 0;
}

在 src/practice1/CMakeLists.txt中添加：

add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server ${PROJECT_NAME}_generate_messages_cpp) # 让这句话先执行以先去生成消息文件

编译。
运行如下命令验证：

roscore
rosrun practice1 add_two_ints_server
rosservice call /add_two_ints 3 5

7.2 客户端 ( add_two_ints_client.cpp)

#include "ros/ros.h"
#include "practice1/AddTwoints.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "add_two_ints_client"); // 初始化节点
    if (argc != 3)
    {
        ROS_INFO("usage: add_two_ints_client X Y");
        return 1;
    }

    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<practice1::AddTwoints>("add_two_ints");
    practice1::AddTwoints srv;
    srv.request.a = atoll(argv[1]);
    srv.request.b = atoll(argv[2]);

    if (client.call(srv))
    {
        ROS_INFO("Sum: %ld", (long int)srv.response.sum);
    }
    else
    {
        ROS_INFO("Failed to call service add_two_ints.");
        return 1;
    }

    return 0;
}

在 src/practice1/CMakeLists.txt中添加：

add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client ${PROJECT_NAME}_generate_messages_cpp) # 让这句话先执行以先去生成消息文件

编译。
运行如下命令验证：

roscore
rosrun practice1 add_two_ints_server
rosservice call /add_two_ints 3 5

8. TF 坐标变换编程

8.1 静态坐标系

1. 新建 src/practice1/launch/tf_transform.launch文件，并写入：

   <launch>
       <node pkg="tf" type="static_transform_publisher" name="static_transform"
           args="0.1 0.0 0.2 0.0 0.0 0.0 /tf1 /tf2 20">
           <!-- args 前三个值表示 xyz 轴方向的偏移 -->
           <!-- args 接着的三个值是欧拉角表示（航向、俯仰、横滚） -->
       </node>
   </launch>
   

2. 运行如下命令：

   roslaunch practice1 tf_transform.launch
   rosrun rqt_tf_tree rqt_tf_tree
   rviz # Add --> TF --> Fix Frame: tf1
   

   
   
3. 修改 src/practice1/launch/tf_transform.launch文件，并写入：

<launch>
    <!-- <node pkg="tf" type="static_transform_publisher" name="static_transform"
        args="0.1 0.0 0.2 0.0 0.0 0.0 /tf1 /tf2 20"> -->
        <!-- args 前三个值表示 xyz 轴方向的偏移 -->
        <!-- args 接着的三个值是欧拉角表示（航向、俯仰、横滚） -->
    <!-- </node> -->
    <node pkg="tf" type="static_transform_publisher" name="static_transform"
        args="0.1 0.0 0.2 0.0 0.0 0.0 1.0 /tf1 /tf2 20">
        <!-- args 前三个值表示 xyz 轴方向的偏移 -->
        <!-- args 接着的四个值是四元数表示 -->
    </node>
</launch>

4. 运行如下命令：

   roslaunch practice1 tf_transform.launch
   rviz # Add --> TF --> Fix Frame: tf1
   

8.2 动态坐标系

相关阅读

[1] ros::spin() 和 ros::spinOnce() 区别及详解

相关参考

[1] 编译器读到"#!/usr/bin/env python"会执行怎样的操作？
[2] [Python]"no encoding declared 错误"解决方法