基于 ROS turtlesim 的简单编程

博文编辑中…

1. ROS demo

2. ROS 简单指令

3. ROS 工程

本节内容基本根据古月居老师的《ROS入门21讲》整理归纳

本节功能包部分代码皆使用python3语言编写

3.1. 工程概述

3.1.1 工程结构

workspace
├── src/
├── build/
├── devel/
└── install/

ROS工程的开发、编译等都需要在ROS的工作空间（workspace）中进行，在工作空间中一般分为四个目录：

src，代码空间（Source Space），放置工程的功能包，所有的功能包代码、配置等都在src中进行；
build，编译空间（Build Space），放置工程编译过程中产生的中间文件；
devel，开发空间（Development Space），放置工程编译生成的可执行文件、库、脚本等；
install，安装空间（Install Space），工程使用catkin_make install目录生成的安装目录，与devel有一定重复。

3.1.2 功能包

功能包是ROS编译的最小单元，其在src代码空间目录下，不能将代码直接放在src目录下进行编译，必须创建工作空间。

创建功能包后，其内部结构如下：

package
├── include/
├── src/
├── CMakeLists.txt
└── package.xml

其作用分别为：

include/，放置功能包的头文件等；
src/，放置功能包的代码；
CMakeLists.txt，其内容为功能包的编译规则，使用基于gcc编译器的CMake语法，它指定了工程编译时需要依赖的库、头文件等，配置了编译工程时的一些设置；
package.xml，其内容描述了功能包基本信息（功能包名、版本、描述、作者信息、许可证信息等）以及功能包的依赖信息，在创建功能包时没有添加的依赖可以在此文件内手动添加，需要注意的是，此处只是描述了功能包的依赖信息，并非指定了编译功能包时需要引用的依赖。

3.2. 创建工程

3.2.1 创建工作空间

mkdir -p ~/first_ws/src
cd ~/first_ws/src
# 初始化工作空间
catkin_init_workspace

3.2.2 创建功能包

1
2
3

cd ~/first_ws/src
# 创建功能包
catkin_create_pkg test_pkg std_msg rospy roscpp

catkin_create_pkg即创建功能包命令，其结构为：

catkin_create_pkg <功能包名> [依赖]

那么本小节创建功能包的代码含义即为：创建了一个名为test_pkg的功能包，其依赖于std_msg、rospy、roscpp。ROS工程若需要调用python、cpp接口，则需要依赖rospy、roscpp，set_msg为ROS定义的标准消息结构。

3.2.3 编译工程

1
2
3

cd ~/first_ws
# 编译命令
catkin_make

catkin_make可在后面跟上install（即catkin_make install）生成安装空间。

3.2.4 配置工程的环境变量

1
2
3

source ~/first_ws/devel/setup.bash
# 检查环境变量
echo $ROS_PACKAGE_PATH

环境变量配置完成后，系统才能通过环境变量找到你编写的功能包。

3.3. 编写几个简单的功能包

需要注意的是，功能包.py代码的第一行应为#!/usr/bin/env python[2]，否则会报出类似can't read /var/mail/geometry_msgs.msg的错误。

建议将python脚本与cpp源代码区分，将.py文件放置在功能包src/scripts（需手动新建）目录下。

需要注意的是，此处的目录只是个人习惯或者约定俗成，并不意味着必须严格按照此结构开发编程

3.3.1 vel_cmd 发布者

本小节的内容是编写一个发布者，向海龟仿真器turtlesim发布速度指令，完成对海龟的简单控制。

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

import rospy
from geometry_msgs.msg import Twist

def publisher():
  # 初始化ros节点
  rospy.init_node('velocity_publisher', anonymous=True)

  # 创建一个publisher，发布名为/turtle1/cmd_vel的topic，消息类型为Twist，队列长度为10
  pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size = 10)

  # 设置循环频率
  rate = rospy.Rate(2)

  flag = 0

  while not rospy.is_shutdown():

    # 初始化Twist类型的消息
    vel_msg = Twist()

    if flag == 0:
      flag = 1
      vel_msg.angular.z = 2.5
    else:
      flag = 0
      vel_msg.linear.x = 2

    # 发布消息
    pub.publish(vel_msg)
    # 输出log
    rospy.loginfo('publish turtle1 cmd_vel: %f, %f.' % (vel_msg.linear.x, vel_msg.angular.z))

    rate.sleep()

if __name__ == '__main__':
  try:
    publisher()
  except rospy.ROSInterruptException:
    pass

和本节最开始描述的一样，脚本第一行为#!/usr/bin/env python[2]。

其中，rate = rospy.Rate(2)与rate.sleep()结合使用，可以保证while循环以2Hz的频率发布topic。

3.3.2 post 订阅者

本小节的内容是编写一个订阅者，订阅turtlesim发布的post消息，获取海龟的位姿信息。

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

import rospy
from turtlesim.msg import Pose

# 订阅者的回调函数
def poseCallback(pose):
  rospy.loginfo('turtles pose: x: %f, y: %f', pose.x, pose.y)

def subscriber():
  # 初始化ros节点
  rospy.init_node('pose_subscriber', anonymous = True)

  # 创建一个subscriber，订阅名为/turtle1/pose的topic，消息类型为Pose，队列长度为10
  rospy.Subscriber('/turtle1/pose', Pose, poseCallback)

  # 循环等待回调函数
  rospy.spin()

if __name__ == '__main__':
  try:
    subscriber()
  except rospy.ROSInterruptException:
    pass

3.3.3 tf 坐标系广播

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

import roslib
roslib.load_manifest('learning_tf')
import rospy
import turtlesim.msg
import tf

def poseCallback(msg, turtle_name):
  br = tf.TransformBroadcaster()

  br.sendTransform(
    (msg.x, msg.y, 0), 
    tf.transformations.quaternion_from_euler(0, 0, msg.theta),
    rospy.Time.now(), 
    turtle_name,
    'world'
  )

if __name__ == '__main__':
  try:
    rospy.init_node('my_tf_broadcaster')

    turtle_name = rospy.get_param('~turtle')
    
    sub = rospy.Subscriber('%s/pose' % turtle_name, turtlesim.msg.Pose, poseCallback, turtle_name)

    rospy.spin()
  except rospy.ROSInterruptException:
    pass

此处运行代码与21讲略有区别，应为：

1	rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=/turtle1

即rospy.get_param('~turtle')处通过_turtle:=<value>获取参数。

3.3.4 tf 监听

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

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import turtlesim.srv
import geometry_msgs.msg

if __name__ == '__main__':
  rospy.init_node('my_tf_listener')

  listener = tf.TransformListener()

  rospy.wait_for_service('/spawn')
  spawner = rospy.ServiceProxy('/spawn', turtlesim.srv.Spawn)
  spawner(4.0, 2.0, 0.0, 'turtle2')

  pub = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)

  rate = rospy.Rate(10)

  while not rospy.is_shutdown():
    try:
      (trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
      continue

    angular = 4 * math.atan2(trans[1], trans[0])
    linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)

    cmd = geometry_msgs.msg.Twist()
    cmd.linear.x = linear
    cmd.angular.z = angular
    pub.publish(cmd)

    rate.sleep()

若此处报错：

 service [/spawn] responded with an error: b''

则是因为turtlesim已存在了一只同名的海龟，应该是重复运行了该脚本导致的。

3.3.5 Launch 启动文件

在功能包的更目录下创建launch子目录（即与src同级）,在将launch启动文件放在次目录下。

需要注意的是，此处的目录只是个人习惯或者约定俗成，并不意味着必须严格按照此结构开发编程

创建launch启动文件：

<launch>
  <node name="sim" pkg="turtlesim" type="turtlesim_node"/>
  <node name="sim_key" pkg="turtlesim" type="turtle_teleop_key" output="screen"/>
  <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
    <param name="turtle" type="string" value="turtle1" /> 
  </node>
  <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
    <param name="turtle" type="string" value="turtle2" /> 
  </node>
  <node name="listener" pkg="learning_tf" type="turtle_tf_listener.py"/>
</launch>