ROS2实战，自定义接口，让代码更优雅

随风1

在上一篇文章《ROS2实战，我写了个AEB程序，在CARLA中进行了仿真测试》中，我们创建了两个节点，实现了车辆信息的发布和订阅。但是聪明的你，一定发现当时的程序非常笨重，简单的车辆信息的发送，竟然用到了三个publisher对象，仅仅是因为发送的数据类型不一样，就要换一个publisher，这也太傻了。还好ROS 2提供了用户自定义接口的方法，可以把这些要发送的信息统一定义到一个接口类型中，然后打包发布，打包订阅。

接口：两个程序之间进行信号对接，所采用的统一的数据格式。

接下来我们看看怎么实现用户自定义接口。

我们还是从零做起，先定义工作空间，再定义功能包，再设计代码，再配置代码，最后编译测试。
1、创建ROS 2工作空间

打开终端创建本次演示案例的工作空间：
cd ~/ROS2_study
mkdir ros2_ws_demo006_custom_interface/
cd ros2_ws_demo006_custom_interface
2、创建功能包

为了创建能够在不同功能包之间都通用的接口，我们把接口也独立出来定义成一个只有接口的功能包。进入工作空间，创建src文件夹来存放源码，在src下创建本次演示所需要的用户自定义的接口功能包。
mkdir src
cd src
ros2 pkg create my_custom_interfaces --build-type ament_cmake

my_custom_interfaces 是新包的名称。请注意，它只能是 CMake 包，但这并不限制我们可以在哪种类型的包中使用消息和服务。我们可以在 CMake 包中创建自定义接口，然后在 C++ 或 Python 节点中使用它。

然后我们进入这个包，删除不必要的文件。
cd my_custom_interfaces/
rm -rf include/ src/

然后创建我们需要的接口文件夹（msg），和其内的接口文件（MyCarlaInterface.msg）:
mkdir msg
touch msg/MyCarlaInterface.msg

注意，到此时，我们的整个工作空间下的目录还非常简单：
ros2_ws_demo006_custom_interface/
└── src
    └── my_custom_interfaces
        ├── CMakeLists.txt
        ├── msg
        │   └── MyCarlaInterface.msg
        └── package.xml

3 directories, 3 files
3、编辑接口文件

文件目录定义好了以后，就可以开始编辑文件内容了。首先要编辑的是我们自定义的接口文件MyCarlaInterface.msg。我这里内容如下：
string ego_id
float64 ego_speed_x
float64 ego_speed_y
float64 ego_speed_z
float64 ego_position_x
float64 ego_position_y
float64 ego_position_z
string obj_id
float64 obj_speed_x
float64 obj_speed_y
float64 obj_speed_z
float64 obj_position_x
float64 obj_position_y
float64 obj_position_z

这些都是我们在上一节文章中要发布和订阅的车辆信号，我这里统一把他们都定义在这个文件里面了。每个信号的格式都是“类型+变量名”，变量名是用户自己定义的，变量类型可以参考如下列表：
bool
byte
char
float32
float64
int8
uint8
int16
uint16
int32
uint32
int64
uint64
string

注意，上面的这些类型都是最最最基础的类型，其实ROS 2还基于这些基础数据继续封装以提供其他更为复杂数据类型，比如std_msgs、geometry_msgs等，它们其实就是对一些比较典型的接口进行了封装，我们接下来要做的就是根据我们自己的需求，做出跟这个差不多的东西来。
4、配置接口文件

打开CMakeLists.txt文件，添加如下内容：
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/MyCarlaInterface.msg"
)

它的作用是定义将我们刚刚创建的接口文件使用rosidl工具编译成可以调用的库文件。

再来看下CMakeLists.txt的完整内容吧：
cmake_minimum_required(VERSION 3.5)
project(tutorial_interfaces)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/MyCarlaInterface.msg"
)

ament_package()

接下来需要配置下package.xml文件，需要新增以下内容：
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

完整的package.xml文件内容如下：
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_custom_interfaces</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="goodman@todo.todo">goodman</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>

  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

</package>

然后我们回到工作空间，编译一下这个接口功能包。
cd ~/ROS2_study/ros2_ws_demo006_custom_interface/
colcon build --packages-select my_custom_interfaces

看到屏幕显示的如下编译信息，就是编译成功了。
Starting >>> my_custom_interfaces
Finished <<< my_custom_interfaces [5.04s]                    

Summary: 1 package finished [7.04s]
5、接口文件检查

接下来我们使用ros2命令行工具检查下当前工作空间下的接口属性吧：
source install/setup.bash
ros2 interface show my_custom_interfaces/msg/MyCarlaInterface

我们看到屏幕上打印出如下的内容：
string ego_id
float64 ego_speed_x
float64 ego_speed_y
float64 ego_speed_z
float64 ego_position_x
float64 ego_position_y
float64 ego_position_z
string obj_id
float64 obj_speed_x
float64 obj_speed_y
float64 obj_speed_z
float64 obj_position_x
float64 obj_position_y
float64 obj_position_z

这可不就是我们定义好的接口文件的内容嘛，它现在已经在工作空间的接口库里了，后面我们就能直接调用了。
6、创建新的功能包

接下来就是对上一篇文章中的代码进行基于新接口的改造。

切换到src目录下，创建新的功能包。
cd src
ros2 pkg create aeb_control_pkg --build-type ament_python --dependencies rclpy

此时，src下的目录树如下：
src/
├── aeb_control_pkg
│   ├── aeb_control_pkg
│   │   └── __init__.py
│   ├── package.xml
│   ├── resource
│   │   └── aeb_control_pkg
│   ├── setup.cfg
│   ├── setup.py
│   └── test
│       ├── test_copyright.py
│       ├── test_flake8.py
│       └── test_pep257.py
└── my_custom_interfaces
    ├── CMakeLists.txt
    ├── msg
    │   └── MyCarlaInterface.msg
    └── package.xml

6 directories, 11 files
7、创建节点

我们来继续定义aeb_control_pkg这个功能包下的节点文件，scenario_runner_node.py和controller_node.py。
cd src/aeb_control_pkg/aeb_control_pkg
touch scenario_runner_node.py
touch controller_node.py

在scenario_runner_node.py中填入以下内容：
import traceback

import rclpy
from rclpy.node import Node
from my_custom_interfaces.msg import MyCarlaInterface
import time
import carla
import traceback

class PublisherNode(Node):

    def __init__(self):
        super().__init__('publisher_node')
        #
        self.carla_world = None
        self.connect_to_carla()
        self.ego_car = None
        self.obj_car = None
        self.ego_speed_x = None
        self.obj_speed_x = None
        self.ego_pos_x = None
        self.obj_pos_x = None
        self.actor_list = []
        self.spawn_actor_ego(position=(0, 28, 1))
        self.spawn_actor_obj(position=(50, 28, 1))
        self.set_ego_speed(x=5.5, y=0, z=0)

        self.logger = self.get_logger()

        #
        self.my_publisher = self.create_publisher(MyCarlaInterface, '/carla/info', 10)
        self.timer = self.create_timer(1, self.publish_vehicle_info)

    def publish_vehicle_info(self):
        self.get_carla_data()
        # print(self.ego_pos_x, self.ego_speed_x, self.obj_pos_x, self.obj_speed_x)
        if None not in (self.ego_pos_x, self.ego_speed_x , self.obj_pos_x, self.obj_speed_x):
            my_carla_info = MyCarlaInterface()
            my_carla_info.ego_id = self.ego_car.id
            my_carla_info.obj_id = self.obj_car.id
            my_carla_info.ego_position_x = self.ego_pos_x
            my_carla_info.ego_position_y = 2.0
            my_carla_info.ego_position_z = 0.0
            my_carla_info.obj_position_x = self.obj_pos_x
            my_carla_info.obj_position_y = 2.0
            my_carla_info.obj_position_z = 0.0
            my_carla_info.ego_speed_x = self.ego_speed_x
            my_carla_info.ego_speed_y = 0.0
            my_carla_info.ego_speed_z = 0.0
            my_carla_info.obj_speed_x = self.obj_speed_x
            my_carla_info.obj_speed_y = 0.0
            my_carla_info.obj_speed_z = 0.0
            # to pulish all message...
            self.my_publisher.publish(my_carla_info)

            self.logger.info('Publishing... ego:position_x={},speed_x={},obj:position_x={},speed_x={}'.format(
                round(my_carla_info.ego_position_x,2),
                round(my_carla_info.ego_speed_x,2),
                round(my_carla_info.obj_position_x,2),
                round(my_carla_info.obj_speed_x,2)))
        else:
            print("debug data:", self.ego_pos_x, self.ego_speed_x, self.obj_pos_x, self.obj_speed_x)

    def connect_to_carla(self):
        carla_client = carla.Client(host="192.168.2.6", port=2000)
        carla_client.set_timeout(10)  # second
        try:
            self.carla_world = carla_client.get_world()
        except RuntimeError as e:
            self.get_logger().error('can not connect to CARLA world.')
            raise e

    def spawn_actor_ego(self, position):
        blueprint_library = self.carla_world.get_blueprint_library()
        # for ego car
        my_bp = blueprint_library.filter("model3")[0]
        trans_loc = carla.Location(x=position[0], y=position[1], z=position[2])
        trans_rot = carla.Rotation(pitch=0, yaw=0, roll=0)
        spawn_point = carla.Transform(trans_loc, trans_rot)
        my_car = self.carla_world.spawn_actor(my_bp, spawn_point)
        # print("ego position:", my_car.get_location())
        self.ego_car = my_car
        self.actor_list.append(my_car)

    def spawn_actor_obj(self, position):
        obj_blueprint_library = self.carla_world.get_blueprint_library()
        # for ego car
        obj_my_bp = obj_blueprint_library.filter("mkz_2020")[0]
        obj_trans_loc = carla.Location(x=position[0], y=position[1], z=position[2])
        obj_trans_rot = carla.Rotation(pitch=0, yaw=0, roll=0)
        obj_spawn_point = carla.Transform(obj_trans_loc, obj_trans_rot)
        obj_my_car = self.carla_world.spawn_actor(obj_my_bp, obj_spawn_point)
        self.obj_car = obj_my_car
        self.actor_list.append(obj_my_car)

    def set_ego_speed(self, x, y, z):
        if self.ego_car:
            # print("ego_car:", self.ego_car.id)
            target_speed = carla.Vector3D(x, y, z)
            # print("target_speed:", target_speed)
            self.ego_car.set_target_velocity(target_speed)

    def destroy_actors(self):
        for actor in self.actor_list:
            actor.destroy()
            self.get_logger().info(f"destroy actor: {actor}")

    def get_carla_data(self):
        if self.ego_car:
            ego_loc = self.ego_car.get_location()
            self.ego_pos_x = ego_loc.x
            ego_spd = self.ego_car.get_velocity()
            self.ego_speed_x = ego_spd.x
            # print("current_speed:", ego_spd)
        if self.obj_car:
            obj_loc = self.obj_car.get_location()
            self.obj_pos_x = obj_loc.x
            obj_spd = self.obj_car.get_velocity()
            self.obj_speed_x = obj_spd.x
        # pass

def main():
    try:
        rclpy.init()
        publisher_node = PublisherNode()
        rclpy.spin(publisher_node)
    except:
        traceback.print_exc()
    finally:
        publisher_node.destroy_actors()
        publisher_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

在controller_node.py中填入以下内容：
import rclpy
from rclpy.node import Node
from my_custom_interfaces.msg import MyCarlaInterface
import carla

class SubscriberNode(Node):

    def __init__(self):
        super().__init__('subscriber_node')
        #
        self.ttc = None
        self.ego_speed_x = None
        self.obj_speed_x = None
        self.ego_position_x = None
        self.obj_position_x = None
        self.logger = self.get_logger()
        self.my_subscription = self.create_subscription(MyCarlaInterface, '/carla/info', self.callback, 10)
        #
        self.carla_world = None
        self.connect_to_carla()
        self.ego_car_id = None

    def callback(self, msg):
        self.logger.info('Received... ego:position_x={},speed_x={},obj:position_x={},speed_x={}'.format(
                round(msg.ego_position_x,2),
                round(msg.ego_speed_x,2),
                round(msg.obj_position_x,2),
                round(msg.obj_speed_x,2)))
        self.ego_car_id = msg.ego_id
        self.ego_speed_x = msg.ego_speed_x
        self.ego_position_x = msg.ego_position_x
        self.obj_speed_x = msg.obj_speed_x
        self.obj_position_x = msg.obj_position_x
        #
        self.calculate_ttc()
        # brake ego
        if self.ttc is not None:
            if 0 < self.ttc < 3:
                if self.ego_car_id is not None:
                    ego_car = self.carla_world.get_actor(self.ego_car_id)
                    brake_control = carla.VehicleControl(brake=1, steer=0, throttle=0)
                    ego_car.apply_control(brake_control)
                    self.logger.info('send braking order to ego car')

    def calculate_ttc(self):
        if None not in (self.ego_speed_x, self.obj_speed_x, self.ego_position_x, self.obj_position_x):
            distance = (self.obj_position_x - self.ego_position_x)
            speed_relative = self.ego_speed_x - self.obj_speed_x
            if speed_relative != 0:
                self.ttc = distance / speed_relative
                # print("ttc:", self.ttc)
                self.logger.info('ttc: {}'.format(self.ttc))
            else:
                self.ttc = 999
        else:
            self.logger.info('ttc: can not be calculated')
            # print(self.ego_speed_x)
            # print(self.obj_speed_x)
            # print(self.ego_position_x)
            # print(self.obj_position_x)
    def connect_to_carla(self):
        carla_client = carla.Client(host="192.168.2.6", port=2000)
        carla_client.set_timeout(10)  # second
        try:
            self.carla_world = carla_client.get_world()
        except RuntimeError as e:
            self.get_logger().error('can not connect to CARLA world.')
            raise e

def main():
    rclpy.init()
    subscriber_node = SubscriberNode()
    rclpy.spin(subscriber_node)
    subscriber_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

8、配置节点

节点文件编写好之后又到了配置阶段了，我们打开setup.py脚本，将两个节点配置进去。打开配置文件'src/aeb_control_pkg/setup.py'，修改’console_scripts‘的内容：加入本次的两个节点，完成后的setup.py完整内容如下：
from setuptools import setup

package_name = 'aeb_control_pkg'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='goodman',
    maintainer_email='goodman@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "scenario_runner_node = aeb_control_pkg.scenario_runner_node:main",
            "controller_node = aeb_control_pkg.controller_node:main"
        ],
    },
)

到这里，源码内容就已经准备好了。

最后再来看看src下的文件目录：
src/
├── aeb_control_pkg
│   ├── aeb_control_pkg
│   │   ├── controller_node.py
│   │   ├── __init__.py
│   │   └── scenario_runner_node.py
│   ├── package.xml
│   ├── resource
│   │   └── aeb_control_pkg
│   ├── setup.cfg
│   ├── setup.py
│   └── test
│       ├── test_copyright.py
│       ├── test_flake8.py
│       └── test_pep257.py
└── my_custom_interfaces
    ├── CMakeLists.txt
    ├── msg
    │   └── MyCarlaInterface.msg
    └── package.xml

6 directories, 13 files
9、编译和试运行

回到工作空间下，编译功能包：
cd ~/ROS2_study/ros2_ws_demo006_custom_interface/
colcon build

编译过程很快，我这里终端打印的消息长这样：
Starting >>> aeb_control_pkg
Starting >>> my_custom_interfaces            
Finished <<< my_custom_interfaces [2.17s]                                                         
Finished <<< aeb_control_pkg [3.52s]         

Summary: 2 packages finished [6.47s]

这样就表示编译完成啦。

接下来就是测试了。

由于两个ROS 2节点都是CARLA的客户端程序，在运行客户端程序前，我们应该先把CARLA的服务端打开。这样客户端后面才能连接连接到服务端。我这边的CARLA的服务端启动程序如下：
cd path/to/your/carla
./CarlaUE4.sh

然后等CARLA服务端完全运行起来后。我们新打开一个终端，输入节点1启动指令：
source install/setup.bash
ros2 run aeb_control_pkg scenario_runner_node

此时，我们应该能从CARLA的服务端看到两辆车被创建了出来，并且主车逐渐靠近前方的目标车。如果没有下方的节点2启动，则主车会与目标车发生碰撞。

然后在新打开一个终端，赶在碰撞前，把AEB控制节点运行起来：
source install/setup.bash
ros2 run aeb_control_pkg  controller_node

然后我们就能看到主车在撞到目标车前就制动停了下来。



屏幕截图
节点1打印的消息如下：



屏幕截图

节点2打印的消息如下：



屏幕截图10、总结

本节我们通过对车辆之间通信消息的自定义，把所有消息用一个接口文件包装了起来，这样在发布和订阅过程中都更加的优雅，逻辑更加清晰，代码简化了不少。然后基于新的接口文件重新优化了上一节的发布和订阅程序，实现了AEB功能的控制。完结，撒花。

ROS 2中比较多使用这种自定义接口的方法，但是采用protobuf对数据进行发送能更轻量级、也能更定制化，但是我一直没有成功。不知道哪位道友有知道做法的可以一起探讨下。