ROS高效入门第四章 -- 自定义ROS消息，编写python pub+sub样例

1 前言

这篇文章是本人上篇博客的姊妹篇： 自定义ROS消息，编写C++ pub+sub样例
见博客名可知，本文主要讲解使用python编写ros pub和sub样例，参考资料也是一样的：
（1）《机器人操作系统（ROS）浅析》[美] Jason M. O’Kane 著 肖军浩 译，第3，5和6章
（2）ros Tutorials 初级教程的10~12节
本系列博客汇总：ROS高效入门系列。

2 正文

2.1 第一个ros程序，hello_ros

（1）创建软件包hello_ros和文件

cd ~/catkin_ws/src/python/
catkin_create_pkg hello_ros std_msgs rospy roscpp

cd hello_ros 
mkdir launch scripts
touch launch/start.launch scripts/hello.py

（2）编写hello.py，CMakeLists.txt，start.launch
hello.py:

#! /usr/bin/env python3

import rospy

def main():
	// 相当于cpp中的ros::init+ros::NodeHandle
    rospy.init_node("hello_ros")    
    rospy.loginfo("hello ros")

if __name__ == "__main__":
    main()

CMakeLists.txt：

cmake_minimum_required(VERSION 3.0.2)
project(hello_ros)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
// 这句是必须要加的，不然hello.py无法安装出来，自然也执行不了
catkin_install_python(PROGRAMS
  scripts/hello.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

start.launch

<launch>
<node
    pkg="hello_ros"
    type="hello.py"
    name="hello_ros"
    required="true"
    output="screen"
/>
</launch>

（3）编译并运行（即使是python版，也是要编译的）

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

source devel/setup.bash
roslaunch hello_ros start.launch

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

（1）创建simple_pub_sub软件包和文件

cd ~/catkin_ws/src/python
catkin_create_pkg simple_pub_sub std_msgs rospy roscpp

mkdir launch scripts
touch launch/start.launch scripts/pub.py scripts/sub.py

（2）编写pub.py，sub.py，CMakeLists.txt，start.launch
pub.py:

#! /usr/bin/env python3

import rospy
from std_msgs.msg import String

def main():
    rospy.init_node("sim_pub")
    // 建立pub句柄，第一个参数是topic的相对名，第二个是类型，第三个是缓存队列长度
    pub = rospy.Publisher("chatter", String, queue_size=10)

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        msg = String()
        msg.data = "hello ycao %s" %rospy.get_time()
        pub.publish(msg)
        rospy.loginfo(msg.data)
        rate.sleep()

if __name__ == "__main__":
    main()

sub.py:

#! /usr/bin/env python3

import rospy
from std_msgs.msg import String

def msg_cb(msg):
    tmp_str = "%s: i received %s" %(rospy.get_caller_id(), msg.data)
    rospy.loginfo(tmp_str)

def main():
    rospy.init_node("sim_sub")
    // 建立sub句柄，第三个参数是回调函数
    rospy.Subscriber("chatter", String, msg_cb)
    // 这里的spin与cpp中的不一样，它只是不让节点推出，跟是否调用回调函数没有关系
    // 而且，rospy没有spinonce()方法
    rospy.spin()

if __name__ == "__main__":
    main()

CMakeLists.txt：

cmake_minimum_required(VERSION 3.0.2)
project(simple_pub_sub)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
catkin_install_python(PROGRAMS
  scripts/pub.py scripts/sub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

start.launch

<launch>
<node
    pkg="simple_pub_sub"
    type="pub.py"
    name="sim_pub"
    respawn="true"
    output="screen"
/>
<node
    pkg="simple_pub_sub"
    type="sub.py"
    name="sim_sub"
    respawn="true"
    output="screen"
/>
</launch>

（3）编译并执行

cd ~/catkin_ws
catkin_make --source src/python/simple_pub_sub/

source devel/setup.bash
roslaunch simple_pub_sub start.launch

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

（1）创建msg_self软件包，和相应文件

cd ~/catkin_ws/src/python
catkin_create_pkg msg_self message_generation rospy roscpp message_runtime

mkdir launch scripts msg
touch launch/start.launch scripts/pub.py scripts/sub.py msg/Student.msg

（2）编写Student.msg，pub.py，sub.py, CMakeLists.txt，start.launch
Student.msg

string name
uint8 age

pub.py

#! /usr/bin/env python3

import rospy
from msg_self.msg import Student

def main():
    rospy.init_node("msg_pub")
    pub = rospy.Publisher("student", Student, queue_size=10)

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        s1 = Student()
        s1.name = "jieshoudaxue"
        s1.age = 30
        pub.publish(s1)
        
        rospy.loginfo("send s1, name = %s, age = %d" %(s1.name, s1.age))
        rate.sleep()

if __name__ == "__main__":
    main()

sub.py

#! /usr/bin/env python3

import rospy
from msg_self.msg import Student

def stu_cb(stu):
    rospy.loginfo("%s: i received s1, name = %s, age = %d" %(rospy.get_caller_id(), stu.name, stu.age))

def main():
    rospy.init_node("msg_sub")
    rospy.Subscriber("student", Student, stu_cb)
    rospy.spin()

if __name__ == "__main__":
    main()

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(msg_self)
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  rospy
)
add_message_files(
  FILES
  Student.msg
)
generate_messages(
  DEPENDENCIES
  std_msgs  # Or other packages containing msgs
)
catkin_package(
 CATKIN_DEPENDS message_runtime roscpp rospy
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
catkin_install_python(PROGRAMS
  scripts/pub.py scripts/sub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

start.launch

<launch>
<node
    pkg="msg_self"
    type="pub.py"
    name="msg_pub"
    respawn="true"
    output="screen"
/>
<node
    pkg="msg_self"
    type="sub.py"
    name="msg_sub"
    respawn="true"
    output="screen"
/>
</launch>

（3）编译并运行

cd ~/catkin_ws/
catkin_make --source src/python/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/python
catkin_create_pkg handle_turtlesim turtlesim geometry_msgs rospy roscpp

mkdir launch scripts
touch launch/start.launch scripts/pub.py scripts/sub.py

（2）编写pub.py，sub.py, CMakeLists.txt, start.launch
pub.py

#! /usr/bin/env python3

import rospy
import random
from geometry_msgs.msg import Twist

def main():
    rospy.init_node("pub_velocity")
    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=10)

    rate = rospy.Rate(2)
    while not rospy.is_shutdown():
        msg = Twist()
        msg.linear.x = random.random()
        msg.angular.z = random.random()
        pub.publish(msg)
        
        rospy.loginfo("sending rand velocity cmd: linear = %s, angular = %s" %(msg.linear.x, msg.angular.z))
        rate.sleep()

if __name__ == "__main__":
    main()

sub.py

#! /usr/bin/env python3

import rospy
from turtlesim.msg import Pose

def pose_cb(msg):
    rospy.loginfo("i received: [%s, %s], direction: %s" %(msg.x, msg.y, msg.theta));

def main():
    rospy.init_node("sub_pose")
    rospy.Subscriber("turtle1/pose", Pose, pose_cb)
    rospy.spin()

if __name__ == "__main__":
    main()

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}
)
catkin_install_python(PROGRAMS
  scripts/pub.py scripts/sub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

start.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    respawn="true"
/>
<node
    pkg="handle_turtlesim"
    type="pub.py"
    name="pub_velocity"
    respawn="true"
    output="screen"
/>
<node
    pkg="handle_turtlesim"
    type="sub.py"
    name="sub_pose"
    required="true"
    output="screen"
/>
</launch>

（3）编译并运行

cd ~/catkin_ws
catkin_make --source src/python/handle_turtlesim/

source devel/setup.bash
roslaunch handle_turtlesim start.launch

3 总结

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