百度Apollo智能驾驶之CyberRT__自定义cyber_param命令行工具

自定义 cyber_param 命令行工具

前言

最近在使用 Apollo 的实时通信框架 CyberRT，难免的使用中会与ROS作比较，就个人感受而言，CyberRT更简洁高效，尤其是当从CyberRT切换回ROS时更是能够感觉到ROS设计的臃肿且冗余，不过简洁的的代价之一就是功能上的弱化，比如：命令行工具无论在CyberRT还是ROS中都是极其常用的，使用命令工具可以大大提高我们的开发调试效率，但是使用过程中发现较之于ROS，在CyberRT中竟然没有提供参数服务相关的命令。咋办？自己写一个呗！闲话少说，先上最终实现效果图。

查询所有参数：

说明:

• 功能: 查询所有参数；
• 语法: cyber_param list;
• 输出: 参数服务的节点名称，以及其包含的参数的变量名称与数据类型。

​

获取指定参数：

说明:

• 功能: 根据参数名称获取参数值；
• 语法: cyber_param get 参数节点 参数名称；
• 输出: 参数值。

修改某个参数：

说明:

• 功能: 根据参数名称修改参数值；
• 语法: cyber_param set 参数节点 参数名称 参数值；
• 输出: 修改成功输出提示信息: [OK!]。

修改成功后，再调用查询指令可以查看修改后的数据。

演示完基本使用效果之后，再介绍一下完整实现流程。

实现

整个实现流程大致如下：

1. 框架搭建

   创建源文件并编辑配置文件，预期结果可以在终端执行指令 cyber_param，终端输出测试用的提示信息。

2. 功能实现

   分别实现参数查询所有、获取指定以及修改的功能。

3. 问题汇总

   关于实现中问题的说明。

1.框架搭建

预期实现结果，在终端输入 cyber_param 不会抛出异常，且给出提示信息，如下图所示：

框架搭建稍显繁琐，不过在 /apollo/cyber/tools 下已经包含了诸如 cyber_node、cyber_channel 等指令，在自定义cyber_param时，可以参考其他指令的实现与配置，主要步骤如下：

1. 新建源文件；
2. 修改 /apollo/cyber/tools 下的配置文件；
3. 修改 /apollo/cyber/setup.bash；
4. 编译；
5. 测试；

1.1新建源文件

/apollo/cyber/tools 目录下新建文件夹:cyber_param。cyber_param 下新建BUILD文件，与 cyber_param.py 文件。

cyber_param.py 文件内容如下:

#!/usr/bin/env python3

from cyber.python.cyber_py3 import cyber

if __name__ == "__main__":
    cyber.init()
    print("test....")
    cyber.shutdown()

BUILD文件内容如下：

load("@rules_python//python:defs.bzl", "py_binary")
load("//tools/install:install.bzl", "install_files")

package(
    default_visibility = ["//visibility:public"],
)

py_binary(
    name = "cyber_param",
    srcs = ["cyber_param.py"],
    deps = [
        "//cyber/python/cyber_py3:cyber",
        "//cyber/python/cyber_py3:parameter",
    ],
)
install_files(
    name = "install",
    dest = "bin",
    files = [
        ":cyber_param.py",
    ],
    rename = {
        "bin/cyber_param.py": "cyber_param",
    },
)

1.2修改 /apollo/cyber/tools 下的配置文件

需要修改 tools 下的 BUILD 与 cyber_tools_auto_complete.bash 这两个配置文件。

BUILD 文件添加 cyber_param 的相关依赖，修改后内容如下：

load("//tools/install:install.bzl", "install")

package(
    default_visibility = ["//visibility:public"],
)

install(
    name = "install",
    deps = [
        "//cyber/tools/cyber_launch:install",
        "//cyber/tools/cyber_monitor:install",
        "//cyber/tools/cyber_recorder:install",
        "//cyber/tools/cyber_channel:install",
        # 自定义
        "//cyber/tools/cyber_param:install",
    ],
)

cyber_tools_auto_complete.bash 文件需要添加如下内容：

......
function _cyber_param_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_param')
    COMPREPLY=( $(compgen -W "list info" -- ${word}) )
    ;;
  *)
    ;;
  esac
}
......
complete -F _cyber_param_complete -o default cyber_param

修改后完整内容如下：

# usage: source cyber_tools_auto_complete.bash

function _cyber_launch_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_launch')
    COMPREPLY=( $(compgen -W "start stop" -- ${word}) )
    ;;
  'start')
    compopt -o nospace
    local files=`ls *.launch 2>/dev/null`
    COMPREPLY=( $(compgen -W "$files" -- ${word}) )
    ;;
  'stop')
    compopt -o nospace
    local files=`ls *.launch 2>/dev/null`
    COMPREPLY=( $(compgen -W "$files" -- ${word}) )
    ;;
  *)
    ;;
  esac
}

function _cyber_recorder_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_recorder')
    COMPREPLY=( $(compgen -W "play info record split recover" -- ${word}) )
    ;;
  *)
    ;;
  esac
}

function _cyber_channel_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_channel')
    COMPREPLY=( $(compgen -W "echo list info hz bw type" -- ${word}) )
    ;;
  *)
    ;;
  esac
}

function _cyber_node_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_node')
    COMPREPLY=( $(compgen -W "list info" -- ${word}) )
    ;;
  *)
    ;;
  esac
}

function _cyber_service_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_service')
    COMPREPLY=( $(compgen -W "list info" -- ${word}) )
    ;;
  *)
    ;;
  esac
}
# 自定义的
function _cyber_param_complete() {
  COMPREPLY=()
  local word=${COMP_WORDS[COMP_CWORD]}
  local cmd=${COMP_WORDS[COMP_CWORD-1]}
  case $cmd in
  'cyber_param')
    COMPREPLY=( $(compgen -W "list info" -- ${word}) )
    ;;
  *)
    ;;
  esac
}
complete -F _cyber_launch_complete -o default cyber_launch
complete -F _cyber_recorder_complete -o default cyber_recorder
complete -F _cyber_channel_complete -o default cyber_channel
complete -F _cyber_node_complete -o default cyber_node
complete -F _cyber_service_complete -o default cyber_service
# 自定义
complete -F _cyber_param_complete -o default cyber_param

1.3修改 /apollo/cyber/setup.bash

setup.bash 中需要设置 cyber_param 相关参数，修改后的内容如下所示（主要参考第19行与27行实现）：

#! /usr/bin/env bash
TOP_DIR="$(cd "$( dirname "${BASH_SOURCE[0]}" )/.." && pwd -P)"
source ${TOP_DIR}/scripts/apollo.bashrc

export APOLLO_BAZEL_DIST_DIR="${APOLLO_CACHE_DIR}/distdir"
export CYBER_PATH="${APOLLO_ROOT_DIR}/cyber"

bazel_bin_path="${APOLLO_ROOT_DIR}/bazel-bin"
mainboard_path="${bazel_bin_path}/cyber/mainboard"
cyber_tool_path="${bazel_bin_path}/cyber/tools"
recorder_path="${cyber_tool_path}/cyber_recorder"
launch_path="${cyber_tool_path}/cyber_launch"
channel_path="${cyber_tool_path}/cyber_channel"
node_path="${cyber_tool_path}/cyber_node"
service_path="${cyber_tool_path}/cyber_service"
monitor_path="${cyber_tool_path}/cyber_monitor"
visualizer_path="${bazel_bin_path}/modules/tools/visualizer"
#自定义
param_path="${cyber_tool_path}/cyber_param"

# TODO(all): place all these in one place and pathprepend
for entry in "${mainboard_path}" \
    "${recorder_path}" "${monitor_path}"  \
    "${channel_path}" "${node_path}" \
    "${service_path}" \
    "${launch_path}" \
    "${param_path}" \
    "${visualizer_path}" ; do
    pathprepend "${entry}"
done

pathprepend ${bazel_bin_path}/cyber/python/internal PYTHONPATH

export CYBER_DOMAIN_ID=80
export CYBER_IP=127.0.0.1

export GLOG_log_dir="${APOLLO_ROOT_DIR}/data/log"
export GLOG_alsologtostderr=1
export GLOG_colorlogtostderr=1
export GLOG_minloglevel=0

export sysmo_start=0

# for DEBUG log
#export GLOG_v=4

source ${CYBER_PATH}/tools/cyber_tools_auto_complete.bash

1.4编译

编译整个 /apollo/cyber/tools 包：

bazel build cyber/tools/...

1.5测试

打开终端，首先载入 setup.bash 文件：

source cyber/setup.bash

然后终端中执行:

cyber_param

如果异常，终端的输出与预期结果类似。

2.功能实现

框架搭建完毕，接下来就可以关注于功能的实现了，主要实现的功能有：

1. 命令使用基本逻辑(根据输入的不同参数做出不同的处理)；
2. 命令使用帮助；
3. 列出所有参数功能；
4. 获取指定参数功能；
5. 修改参数功能。

2.1编码

cyber_param.py的代码如下：

#!/usr/bin/env python3

import sys
import os

from cyber.python.cyber_py3 import cyber
from cyber.python.cyber_py3 import parameter
from optparse import OptionParser


def _usage():
    print("""cyber_param is a command-line tool to show information about CyberRT Parameters.

Commands:
\tcyber_param list \tList parameters.
\tcyber_param set \tset parameter.
\tcyber_param get \tget parameter.

Type cyber_param <command> -h for more detailed usage, e.g. 'cyber_param list -h'
""")
    sys.exit(getattr(os, "EX_USAGE", 1))

""" 
    获取所有参数服务节点名称 
        1.通过 cyber.ServiceUtils 获取所有服务节点
        2.参数服务名称的特点是 xxx/get_parameter xxx/list_parameter xxx/set_parameter
          可以根据此特性获取 xxx (也即参数服务节点的名称)
"""
def get_param_service_list():
    names = []
    # 先获取所有的服务名称
    services = cyber.ServiceUtils.get_services()
    # 筛选出参数服务的名称
    for service_name in services:
        # 注意此处的筛选条件并不严谨
        if service_name.endswith("/get_parameter"):
            index = service_name.rindex("/")
            names.append(service_name[:index])
    # 返回名称列表
    return names
"""  
    判断参数服务名称是否正确

"""
def parameter_server_node_name_is_right(name):
    # 获取所有参数服务名称
    param_service_names = get_param_service_list()
    # 判断指定的参数服务名称是否合法
    if name not in param_service_names:
        print("parameterServerNodeName not right")
        return False
    return True

# 打印所有参数列表
def print_param_list(client_node):
    param_service_names = get_param_service_list()
    for ps_name in param_service_names:
        #为每一个参数服务创建对应的参数客户端，并返回参数列表
        param_client = parameter.ParameterClient(client_node,ps_name)
        params = param_client.get_paramslist()
        print(ps_name + ":")
        for param in params:
            print("\tname:%s\ttype:%s"%(param.name().decode("utf-8"),param.type_name().decode("utf-8")))

# list 指令语法合法性判断
def _param_cmd_list(argv,client_node):
    #对列表切片
    args = argv[2:]
    parser = OptionParser(usage="usage: cyber_param list")
    (options,args) = parser.parse_args(args)
    # 判断是否有多于参数
    if len(args) > 0:
        parser.error("too many arguments")
    print_param_list(client_node)

"""  
    打印指定参数服务下的指定参数
"""
def print_param(args,client_node):
    """  
        args[0] 为参数服务名称
        args[1] 为要解析的参数名称
    """
    # 如果参数服务名称不存在直接返回
    if not parameter_server_node_name_is_right(args[0]):
        return
    # 参数客户端创建
    param_client = parameter.ParameterClient(client_node,args[0])
    # 解析参数 --- 当 parameterName不存在时，直接抛出异常
    param = param_client.get_parameter(args[1])
    # 判断参数类型并输出参数值
    type_name = param.type_name().decode("utf-8")
    if type_name == "INT":
        print(param.as_int64())
    elif type_name == "DOUBLE":
        print(param.as_double())
    elif type_name == "STRING":
        print(param.as_string().decode("utf-8"))

def _param_cmd_get(argv,client_node):
    # 对列表切片(命令调用格式: cyber_param get car_param height)
    args = argv[2:]
    parser = OptionParser(usage="usage: cyber_param get parameterServerNodeName parameterName")
    (options, args) = parser.parse_args(args)
    # 必须刚好提交参数服务节点名称与参数名称
    if len(args) > 2:
        parser.error("too many arguments")
    elif len(args) < 2:
        parser.error("parameterServerNodeName parameterName must be specified")
    print_param(args,client_node)

"""  
    为指定的参数服务新建参数或修改参数。
    如果参数不存在，那么就新建参数，且参数值默认为STRING
    如果参数已经存在，那么就修改参数(修改时，必须保证参数数据类型相匹配)
"""
def param_set(args,client_node):
    """  
        args[0] 为参数服务节点
        args[1] 为参数名称
        args[2] 为参数值
    """
    # 判断参数服务名称是否合法
    if not parameter_server_node_name_is_right(args[0]):
        return
    # 创建参数客户端
    param_client = parameter.ParameterClient(client_node,args[0])
    # 获取参数列表
    params = param_client.get_paramslist()
    tmp = args[2]
    for param in params:
        # 判断指令中的参数是否已经存在
        if args[1] == param.name().decode("utf-8"):
            # 判断参数值的数据类型与已经存在的参数数据类型是否一致
            # print(param.type_name())
            type_name = param.type_name().decode("utf-8")
            try:
                if type_name == "INT":
                    tmp = int(args[2])
                elif type_name == "DOUBLE":
                    tmp = float(args[2])
            except Exception as e:
                print(e.args)
                return

    # 如果参数不存在，那么直接添加参数
    param_client.set_parameter(parameter.Parameter(args[1],tmp))
    print("[OK!]")
    

def _param_cmd_set(argv,client_node):
    # 对列表切片(命令调用格式: cyber_param set car_param height 1.80)
    args = argv[2:]
    parser = OptionParser(usage="usage: cyber_param set parameterServerNodeName parameterName parameterValue")
    (options,args) = parser.parse_args(args)
    if len(args) > 3:
        parser.error("too many arguments")
    elif len(args) < 3:
        parser.error("parameterServerNodeName parameterName parameterValue must be specified")
    param_set(args,client_node)

if __name__ == "__main__":

    if len(sys.argv) == 1:
        _usage()
    
    cyber.init()
    # 创建节点
    client_node = cyber.Node("param_client_node")
    argv = sys.argv[0:]
    command = argv[1]

    if command == "list":
        _param_cmd_list(argv,client_node)
    elif command == "get":
        _param_cmd_get(argv,client_node)
    elif command == "set":
        _param_cmd_set(argv,client_node)
    else:
        _usage()
        
    cyber.shutdown()

本来想分步骤逐一实现每一个步骤的，但是那样做不方便排版，所以直接贴了完整代码，且代码中有详细注释，应该没有阅读困难。

2.2测试

案例实现之后，接下来就可以做简单测试了，但是测试时，至少要启动一个参数服务节点，具体测试过程此处略。

问题

虽然cyber_param 的简单功能基本已经实现了，但在实现过程中也发现了一些问题，比如：

cyber_param 命令功能的实现，依赖于参数客户端对象，而该对象的创建又依赖于参数服务节点的名称，那么如何获取参数服务节点名称呢？我们知道CyberRT中的参数服务本身是对服务通信的封装，当创建一个节点名称为xxx的参数服务时，其实就是创建了三个名为 xxx/get_parameter、xxx/list_parameter、xxx/set_parameter 的服务端，因此我们在上述实现中也是根据此规则在所有节点中筛选参数服务节点的，但是这样做其实是存在隐患的，假设我创建了多个服务通信，且自定义的服务通信话题的命名恰巧与参数服务的命名规则一致，那必然将导致节点获取异常。

那么应该如何解决上述问题呢？

在 CyberRT 中提供了诸如ChannelUtils、NodeUtils、ServiceUtils等这些工具类，可以分别获取channel、node、service的一系列信息，那么后续是否也可以为参数服务提供类似的工具类呢？

总结

如果你既了解ROS又了解CyberRT的话，应该不难发现在命令行工具集这方面，无论是命令类型，或是相同类型命令下的功能点，ROS要远比CyberRT丰富，当然这种丰富也是ROS较为臃肿的体现之一，不过臃肿之下也不乏一些极其实用的功能，比如:关于包管理ROS提供了rospack，关于话题通信消息ROS提供了rosmsg，关于服务通信消息ROS提供了rossrv，关于参数操作的rosparam等等，而在CyberRT中则没有类似的实现。又比如ros中话题通信相关命令rostopic提供了发布话题消息的功能，服务通信相关命令rosservice则提供了请求服务的功能，显而易见的我们可以通过这些指令而非编码的方式方便快捷的实现话题发布方或服务客户端，这在调试过程中是极其有帮助的，而在CyberRT中也还没有类似的实现。

当然如同cyber_param一样，我们也可以考虑参考ROS自实现这些功能，但是对于大多数功能而言，在CyberRT中似乎并没有相关的接口，且如果提供相关接口的话，那就意味着要先实现一些特定的库支持，用于解决诸如如何通过类似于反射的方式获取类的问题，那么这就将是一项工作量可观且富有挑战性的任务了。