ROS 第二讲 基本操作

在开始编写ROS程序时, 首先需要构建一个工作空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

cd ~/catkin_ws
catkin_make

source devel/setup.bash

echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
echo "source /home/usrname/catkin_ws/devel/setup.bash" >> ~/.bashrc

这样工作空间就构建完成. 其中大部分都linux相关的操作, 其中主要起构建工作空间作用的有两句: catkin_init_workspace 作用是初始化工作空间; catkin_make 进行编译, 编译工作一定在工作空间的根目录,此处即为catkin_ws.

工作空间构建完成后,还需要在空作空间内构建包即package, 然后在package内编写所需功能模块的代码.比如

cd ~/catkin_ws/src
catkin_create_pkg practice1 std_msgs roscpp rospy

构建功能包需在src目录下进行, 其一般格式: catkin_create_pkg [package_name][dependency1] [dependency2] ... [dependencyN]. package 后面是接下来构建包可能用到的依赖包, 这里需要注意的是依赖包后面仍有办法继续添加,因此写不全也没有问题,只不过后面操作稍稍麻烦一点. 构建包仍需编译, 如上所述, 先回到工作空间根目录:

cd ~/catkin_ws/
catkin_make

catkin_make是对工作空间中的所有package进行编译,也可以指定某一个包进行单独编译: catkin_make --pkg [package_name]

创建ROS nodes

现在开始,编写ROS程序,在这里我们主要以python为主, python编写的代码需要放在script目录下,这个目录目前没有,你需要自己创建.

cd ~/catkin_ws/src/practice1
mkdir script

作为第一个程序, 还是以'hello world' 开始, (ROS可支持多种语言,其中主要的是C++与python, 接下来程序如无特别要求, 都是用python来编写的.) 这里需要两个程序(publisher.py, subscriber.py),一个作为发布者发布信息, 另一个则为作为订阅者订阅信息.

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
import rospy as ros
from std_msgs.msg import String
import time


def msg_publisher():
    ros.init_node('msg_publisher', anonymous=False)
    pub = ros.Publisher('helloworld', String,queue_size=1)
    rate = ros.Rate(10)

    while not ros.is_shutdown():
        strings = 'hello world, now is %s' % str(time.time())
        msg = String()
        msg.data = strings

        pub.publish(msg)
        rate.sleep()


if __name__ == '__main__':
    msg_publisher()

开头引入所需的功能包, rospy 是使用python 来编写ROS程序必不可少的包, std_msgs 是标准信息包,在无特别需求的情况下, 此包基本满足程序所需的信息格式要求.

ros.init_node 是初始化此发布节点, msg_publisher 是此节点的名字, anonymous 设置为True, 意味着本节点可以同时启动多个示例, 反之只能启动一个,否则会因冲突而报错.

ros.Publisher() 是在声明此发布节点发布的topic, topic的名字是helloworld, 此topic数据传输所使用的格式为String类型, queue_size 是缓存的队列长度, 设置一个很小的值即可,比如此处设为1.

ros.Rate(10) 是信息发布的频率, 此处是每秒 10 次.

接下来即是此节点开始发布信息,这是设置在一个循环循环里面的, ros.is_shutdown 表示只要不关闭节点就会一直发布. 因为helloworld 所需的数据格式为String类型,因此需要先将节点发布的信息定义为String格式,即代码中的msg = String(). String类型只有一个data属性,将所需发布的信息赋值于它即可. rate.sleep() 是在控制发布频率.

PS: 最好每一次编写python程序都将如下字段加上,否则可能会报错:

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

下面是订阅程序:

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
import rospy as ros
from std_msgs.msg import String
import time


def call_back(msg):
    print(msg.data)
    return msg.data

def msg_subscriber():
    ros.init_node('msg_subscriber',anonymous=False)
    sub = ros.Subscriber('helloworld',String,callback=call_back)
    ros.spin()


if __name__ == '__main__':
    msg_subscriber()

call_back(msg) 是订阅者处理接收发布者发布信息所需的功能模块. 订阅者订阅的是某个topic, 因此需要知道topic的名称, 然后才可订阅,接收到消息. 此处订阅者订阅的是helloworld topic. ros.spin() 是回调监听函数, 一般写在程序最后,因为,程序执行到这句就会一直在此处循环监听topic,后来的语句执行不到.而且,callback函数也是此处受其调用的,即在此处开始执行,这样,订阅者就可一直接收消息,反映在此例,即你会接收很多句类似'hello world, now is ...'的信息.

因为是python程序写完即可执行, 但为了ROS可以找到写的两个文件, 需要设置其为可执行属性:

cd ~/catkin_ws/practice1/script
chmod +x publisher.py
chmod +x subscriber.py

此处的命令需要在script目录下执行.

接下来开始执行代码:

roscore
rosrun practice1 publisher.py
rosrun practice1 subscriber.py

以上三句分别在三个不同的terminal窗口执行. 切记在执行任何ROS之前都需执行roscore. 执行后,会在subscriber 窗口看到类似的如下信息:

再开启一个窗口使用如下命令查看当前topics:

rostopic list

会显示如下信息:

其中的/helloword即是此处publisher发布的topic,另外两个是log相关的topic.

定制信息类型

在ROS中传输信息, 不同任务一般会有不同的信息格式, 简单的格式可能潢足不了需求, 因此就需要根据任务来定制自己的信息类型.在ROS中, 信息类型的定义是放在msg目录内(需要在包目录下创建即本例中的practice1下面), 并且文件以.msg为后缀. 比如定义一个简单的位置信息. 比如名为 position_msg.msg.

float32 X
float32 Y
float32 Z

这样就定义完成, 位置信息由三个维度定义, 变量类型float32类型.

定制化信息可以被ROS识别依赖信息生成模块, 需要在package.xml中加入如下两句(其实文件内有此两句,只是被注释掉了,找到并解注释即可):

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

还需要在CMakeList.txt中的find_package()中加入message_generation:

find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
)

以及:

## Generate messages in the 'msg' folder
add_message_files(
FILES
position_msg.msg
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)

最后, 开始编译:

cd ~/catkin_ws/
catkin_make --pkg practice1

编译完成后, 可以用rosmsg来查看:

rosmsg show practice1/position_msg

可看到如下信息:

接下来,就可以使用刚刚定义的信息类型了. 如之前,编写publisher与subscriber,可分别命名为msg_pub.py'与msg_sub.py:

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

import rospy as ros
from practice1.msg import position_msg

def msg_publisher():
    ros.init_node('msg_publisher', anonymous=True)
    pub = ros.Publisher('position', position_msg,queue_size=1)
    rate = ros.Rate(10)

    while not ros.is_shutdown():
        msg = position_msg()
        msg.X = 23.
        msg.Y = 12.
        msg.Z = 0.2
        pub.publish(msg)
        rate.sleep()


if __name__ == '__main__':
    msg_publisher()

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

import rospy as ros
from practice1.msg import position_msg


def call_back(msg):
    print('position is X: %s,	Y: %s,	Z: 	%s' % (msg.X, msg.Y, msg.Z))
    return msg.data


def msg_subscriber():
    ros.init_node('msg_subscriber', anonymous=False)
    sub = ros.Subscriber('position', position_msg, callback=call_back)
    ros.spin()


if __name__ == '__main__':
    msg_subscriber()

编写后(以后每写一个python执行程序都需要如此):

cd ~/catkin_ws/practice1/msg
chmod +x msg_pub.py
chmod +x msg_sub.py

开始运行, roscore一定是要首先打开的, 如果之前打开了,就不用再次开启了:

rosrun practice1 msg_pub.py
rosrun practice1 msg_sub.py

可看到如下信息:

Z的变化是精度问题,目前先不用纠结.

Service

前面介绍过,服务是客户端发送请求, 服务端经过处理反回结果的一种更高级的topic类型. 如上面的信息格式, 要使用服务, 需要先定义服务的数据类型. 比如简单的测距服务, 首先在包目录下创建srv目录,然后在里面加入distance_srv.srv文件,文件内容如下:

float32 X
float32 Y
float32 Z
---
float32 DIST

文件中 --- 是分隔符, 其上定义请求所需的数据格式,其下为结果数据格式.

然后, 如上面的定制化message过程, 需要在package.xml中加入(如果上面做过如下操作,不需要重复操作):

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

类似地:

find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
)

## Generate services in the 'srv' folder
add_service_files(
FILES
distance_srv.srv
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)

最后编译(记住,是在包的目录下):

cd ~/catkin_ws/
catkin_make

r查看:

rossrv show practice1/distance_srv

可看到如下信息:

编写服务的程序, 服务的程序两端分别是服务端,客户端, 客户端发送请求, 服务端根据客户端的信息进行处理,产生结果. 先编写服务端代码(distance_server.py):

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

import math
import rospy as ros
from practice1.srv import distance_srv, distance_srvResponse

CURRENT_POSITION = [0.0, 0.0, 0.0]


def call_back(request):
    distance = math.sqrt((request.X - CURRENT_POSITION[0]) ** 2 + (request.Y - CURRENT_POSITION[1]) ** 2 + (
            request.Z - CURRENT_POSITION[2]) ** 2)
    return distance_srvResponse(distance)


def server():
    ros.init_node('distance_server', anonymous=True)
    srv = ros.Service('distance', distance_srv, handler=call_back)
    ros.spin()


if __name__ == '__main__':
    server()

然后是客户端代码(distance_client.py):

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

import rospy as ros
from practice1.srv import distance_srv, distance_srvRequest


def distance_client(X, Y, Z):
    ros.init_node('distance_client', anonymous=True)
    ros.wait_for_service('distance')
    service_proxy = ros.ServiceProxy('distance', distance_srv)
    distance = service_proxy(X, Y, Z)
    ros.loginfo('the distance between target and myself is %s.', distance.DIST)
    return distance.DIST


if __name__ == '__main__':
    X = 1.0
    Y = 2.0
    Z = 3.0
    distance_client(X, Y, Z)

执行后会有类似如下结果:

ROS launch

上面的几个例子,每个只涉及一个topic, 两人个节点,但在实际项目中,会有多个topics, 多个节点, 依次启动非常麻烦. 为此, ROS提供一种.launch文件, 此文件类型启动配置文件, 即当配置好.launch文件并运行,可同时启动多个节点.

若要使用这种方便的启动方式,首先需要在包目录下面创建launch文件夹,并在其中创建.launch文件.

比如上面service的例子, 可以创建distance.launch文件:

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

.launch 其实xml格式的文件, 其中的内容需要放在<launch> </launch>里面. 其中<node name>是节点的名字, pkg 的值是包的名字, 而type的值文件的名字, 而output="screen" 则表明将打印信息输出到当前console中. 这只是.launch文件支持的几个常用的参数, 还有很多,如'para', 'arg','respawn' 等等, 后面接触再介绍用法.

启动.launch文件需要roslaunch 命令, 其格式:

roslaunch pkgname launch_file

此处例子的启动方式即为:

roslaunch practice1 distance.launch

可看到如下类似结果:

最后一行是生的log文件,可先不用管它.

Parameter server and dynamic parameters

参数服务器是ROS Master 的一部分, 其保存的配置信息,可以让所有节点访问. 配置信息在ROS系统运行时也可能需要改变. 改变参数服务器的配置信息需要dynamic_reconfigure包.

配置信息需要放在cfg目录(在包目录下创建)下,在cfg里面创建一个比如叫firsttry.cfg的文件:

#!/usr/bin/env python
PACKAGE = "sample" # your package name

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("int_param",    int_t,    0, "An Integer parameter", 50,  0, 100)
gen.add("double_param", double_t, 0, "A double parameter",    .5, 0,   1)
gen.add("str_param",    str_t,    0, "A string parameter",  "Hello World")
gen.add("bool_param",   bool_t,   0, "A Boolean parameter",  True)

size_enum = gen.enum([ gen.const("Small",      int_t, 0, "A small constant"),
                       gen.const("Medium",     int_t, 1, "A medium constant"),
                       gen.const("Large",      int_t, 2, "A large constant"),
                       gen.const("ExtraLarge", int_t, 3, "An extra large constant")],
                     "An enum to set size")

gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)

exit(gen.generate(PACKAGE, "para", "firsttry"))

可以看到,虽然文件以cfg为后缀,但其实是一个python文件.

首先引入dynamic_reconfigure包内参数生成的所需模块. 然后初始化一个参数生成器,接下来就需要定义参数. 生成器的.add函数可以完成此功能, 其一般格式:

gen.add(name,paramtype, level, description, default=None,min=None,max=None,edit_method='')

• name: 参数名字;
• paramtype:参数类型;
• level: 传入到回调函数内的位掩码;
• description: 参数的描述信息;
• default: 参数默认值;
• min: 参数能达到的最小值;
• max:参数能达到的最大值;
• edit_method: 参数需要额外处理的编辑方法, 比如上面遍历类型数据的映射.

最后一句

exit(gen.generate(PACKAGE, "pracetice1", "firsttry"))

表示生成参数后即退出生成程序. 其中最后一个参数的值一定是此文件名去掉后缀的结果,在此例中,即firstry.cfg, 否则会有奇怪的错误.

因为是python文件, 因此需要赋权:

chmod a+x firsttry.cfg

然后还需要在CMakerLists.txt中加入如下内容:

find_package(catkin REQUIRED COMPONENTS
  dynamic_reconfigure
)

generate_dynamic_reconfigure_options(
  cfg/firsttry.cfg
)

然后编译.

配置参数设定好后,需要创建配置服务器节点(比如命名为cfg_server.py), 使得其它节点可以使用,更改配置参数.

#!/usr/bin/env python
import rospy as ros

from dynamic_reconfigure.server import Server
from practice1.cfg import firsttryConfig


def callback(config, level):
    ros.loginfo("""Reconfigure Request: {int_param}, {double_param}, 
          {str_param}, {bool_param}, {size}""".format(**config))
    return config


if __name__ == "__main__":
    ros.init_node("practice_cfg", anonymous=False)

    srv = Server(firsttryConfig, callback)
    ros.spin()

此程序非常简单, 配置参数的引入此处:from practice1.cfg import firstryConfig, 其一般格式如下:

from [package].cfg import [config_filename]Config.

回调函数callback() 需有level作为入参, 因为本例非常简单,而没有使用此参数, 另外, 需要返回config.

赋权后启动,即其节点即可访问, 我们甚至可以用rqt来显示图形化界面,进行手动调节:

rosrun practice1 cfg_server.py
rosrun rqt_reconfigure rqt_reconfigure

可以看到类似如下界面:

在ROS系统进行时,希望其可以自动获取,更改配置参数. dynamic_reconfigure包的client接口提供了这样的功能.

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

import rospy as ros
import dynamic_reconfigure.client


def callback(config):
    ros.loginfo("Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}".format(**config))


if __name__ == "__main__":
    ros.init_node("cfg_client")

    # name='practice_cfg',name of the server to connect to (usually the node name) 
    client = dynamic_reconfigure.client.Client(name = "practice_cfg", timeout=30, config_callback=callback)

    r = ros.Rate(0.5)
    x = 0
    b = False
    while not ros.is_shutdown():
        x = x + 1
        if x > 10:
            x = 0
        b = not b
        client.update_configuration(
            {"int_param": x, "double_param": (1 / (x + 1)), "str_param": str(ros.get_rostime()), "bool_param": b,
             "size": 1})
        r.sleep()

对于上面的server程序,可以看到其回调函数不需要level作为入参, 也没有要求一定要返回config. 然后我们简单地每0.5秒更新配置参数.运行后,可看到类似如下结果:

也可以获取配置参数, 可用类似如下命令:

cfg = client.update_configuration()
x = cfg.int_param
y = cfg.bool_param
...

Action

相对message, service 在请求后可以得到结果, 即节点间或者说server与client间存在一定的交互, 当更复杂一点的场景, 比如结果不能马上返回,要一等相对较长的时间; 服务当前不可用, 客户端在中途希望取消或重新请求服务等等 service就不一定合适了.再举个更具体的例子,比如机器人导航到某一特定地点, 这个距离可能比较远, 而且希望在行进过程中,都能够知道机器人的位置,或者希望在经过特殊环境比如河流,取消去向目标地点的导航任务.这样的场景, 单单在最终给出结果显然不能令人满意. ROS的actionlib包可以创建action这样的服务类型, 此类服务在执行过程中可以服务端可以输出反馈信息,也而客户端也可在达到既定目标之前可以取消或者重新设定目标.

比如一个简单的计算Fibonacci数列,输入数长度,返回数结果. 首先,在包目录下创建action目录,并在其中创建.action数据类型, 比如叫就叫fibonacci.action:

int32 order
---
int32[] sequence
---
int32[] sequence

.action文件分为三段, 第一段是目标的数据类型定义,比如本例中,即为想要生成Fibonacci数列的长度. 第二部分定义结果数据类型,最后一段则是反馈时的数据类型.

在CMakeList.txt中加入(下面内容一部分已经在CMakeList.txt文件中, 把注释的去掉,缺少的部分加下即可):

find_package(catkin REQUIRED COMPONENTS
  actionlib
  actionlib_msgs
)

add_action_files(
   FILES
   fibonacci.action
)

generate_messages(
  DEPENDENCIES
  actionlib_msgs
)
# below is not essential
# catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES practice1
#  CATKIN_DEPENDS  actionlib_msgs
#  DEPENDS system_lib
# )

如果在创建包时没有加入actionlib与actionlib_msgs依赖,则需要在package.xml中加入如下内容:

  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>

  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend>

然后编译.

我们想要实现的功能是这样的, 输入Fibonacci数列长度(即目标), 服务器为们生成这样的一个数列, 如果长度太大,服务器会拒绝执行,或者当数列的最大值大于某个值时, 客户端就会放弃目标.基于此,始编写action的server与client(比如分别叫做fibonacci_server.py与fibonacci_client.py):

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
import rospy as ros
import actionlib
from practice1.msg import fibonacciAction, fibonacciFeedback, fibonacciResult


class FibonacciActionServer(object):

    def __init__(self):
        ros.init_node('fibonacci_server')
        self.server = actionlib.SimpleActionServer('fibonacci_sequence_gen', fibonacciAction, execute_cb=self.call_back, auto_start=False)
        self.rate = ros.Rate(10)
        self.server.start()
        ros.spin()

    def call_back(self, goal):
        result = fibonacciResult()
        if goal.order <= 1:
            tmp = [0]
        elif goal.order > 45:
            tmp = []
            result = fibonacciResult()
            result.sequence = tmp
            self.server.set_aborted(result,'the order should not more than 45!')
            return
        else:
            tmp = [0, 1]
            feedback = fibonacciFeedback()
            while len(tmp) < goal.order:
                if self.server.is_preempt_requested():
                    result.sequence = tmp
                    self.server.set_preempted(result,'fibonacci preempted.')
                    return
                tmp += [sum(tmp[-2:])]
                feedback.sequence = tmp
                self.server.publish_feedback(feedback)
                self.rate.sleep()

        result.sequence = tmp
        self.server.set_succeeded(result, 'the complete fibonacci get!')


if __name__ == '__main__':
    FibonacciActionServer()

ROS的action实现需要actionlib包, 之前.action数据格式编译生会生成相应的msg方法,即上面的fibonacciAction, fibonacciFeedback, fibonacciResult . action server由action.SimpleActionServer(name, ActionSpec,execute_cb=None,auto_start=False)构建.

其中name是服务器名字, ActionSpec 动作的数据格式, execute_cb是回调函数, auto_start=False一定设置为False, 这是ROS设计之出的瑕疵,不用管它. ros.Rate(10)设置的是反馈的频率, 之前见过多次了. 然后是开启服务器,即self.server.start(), ros.spin()以上过程在这里才开始真正执行, 并持续监听.

在call_back()中即是服务器的计算逻辑了, Fibonacci数列的计算逻辑. 从程序可以看到, 当序列长度的设定大于45服务器拒绝执行(这里主要考虑的是int32溢出的情况),然后在计算时,每计算一次就会发送一次反馈即语句self.server.publish_feedback(). 这里还会判断客端是否中断目标,即self.server.is_preempt_requested(), 如里为真, 那么服务器就会中止计算, 并发送结果即self.server.set_preempted(result,'fibonacci preempted.') 其中第二个参数是消息文本.

如果一切顺利执行,结束后,服务器会发送最终结果,即self.server.set_succeeded(result, 'the complete fibonacci get!').

下面是客户端这边的程序:

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
import rospy as ros
import actionlib
from practice1.msg import fibonacciAction, fibonacciGoal
import sys

class Client(object):
    def __init__(self, goal):
        ros.init_node('fibonacci_client')
        self.client = actionlib.SimpleActionClient('fibonacci_sequence_gen', fibonacciAction)
        self.client.wait_for_server()
        self.max_num = 0
        self.feedback_order = 0
        self.rate = ros.Rate(1000)
        self.goal = fibonacciGoal()
        self.goal.order = goal
        self.client.send_goal(self.goal, feedback_cb=self.callback)
        self.watch()

    def callback(self, feedback):
        self.max_num = feedback.sequence[-1]
        self.feedback_order = len(feedback.sequence)
        print('fibonacci: ', feedback.sequence)

    def watch(self):
        while self.feedback_order < self.goal.order and not self.client.get_result():
            if self.max_num > 100000000:
                self.client.cancel_goal()
                self.client.wait_for_result()
                print('[Result] state: ', self.client.get_state())
                print('[Result] status: ', self.client.get_goal_status_text())
                print('[Result] result: ', self.client.get_result())
                return
            self.rate.sleep()

        self.client.wait_for_result()
        print('[Result] State: %d' % (self.client.get_state()))
        print('[Result] Status: %s' % (self.client.get_goal_status_text()))
        print('[Result] sequence: {}'.format(self.client.get_result().sequence))

if __name__ == '__main__':
    t = sys.argv[-1]
    Client(int(t))

类似地, 客户端由actionlib.SimpleActionClient(name,ActionSpec)构建,只不过这里的name指的是服务器的名字. 构建好客户端后,需要连接服务器,即self.client.wait_for_server(), self.goal = fibonacciGoal(), self.goal.order = goal 与self.client.send_goal(self.goal, feedback_cb=self.callback)是在设定目标并发送目标到服务器. 回调函数主要是对反馈进行处理. self.watch()是监听函数, 其中这里可以简单的用self.client.wait_for_result()来替代.只不过,使用监听函数,可以实现更加复杂的功能,比如这里的当Fibonacci数列中的最大值超过100000000时,即取消目标.

分别在三个窗口启动roscore,rosrun practice1 fibonacci_server.py 以及rosrun practice1 fibonacci_client.py 12 可以看到如下结果:

试将目标设为43可以得到类似如下结果:

如果设为50,则会得到: