在上一篇文章《ROS2实战，我写了个AEB程序，在CARLA中进行了仿真测试》中，我们创建了两个节点，实现了车辆信息的发布和订阅。但是聪明的你，一定发现当时的程序非常笨重，简单的车辆信息的发送，竟然用到了三个publisher对象，仅仅是因为发送的数据类型不一样，就要换一个publisher，这也太傻了。还好ROS 2提供了用户自定义接口的方法，可以把这些要发送的信息统一定义到一个接口类型中，然后打包发布，打包订阅。接口：两个程序之间进行信号对接，所采用的统一的数据格式。接下来我们看看怎么实现用户自定义接口。我们还是从零做起，先定义工作空间，再定义功能包，再设计代码，再配置代码，最后编译测试。1、创建ROS 2工作空间打开终端创建本次演示案例的工作空间：cd ~/ROS2_studymkdir ros2_ws_demo006_custom_interface/cd ros2_ws_demo006_custom_interface 2、创建功能包为了创建能够在不同功能包之间都通用的接口，我们把接口也独立出来定义成一个只有接口的功能包。进入工作空间，创建src文件夹来存放源码，在src下创建本次演示所需要的用户自定义的接口功能包。mkdir srccd srcros2 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 msgtouch msg/MyCarlaInterface.msg 注意，到此时，我们的整个工作空间下的目录还非常简单：ros2_ws_demo006_custom_interface/└── src └── my_custom_interfaces ├── CMakeLists.txt ├── msg │ └── MyCarlaInterface.msg └── package.xml3 directories, 3 files 3、编辑接口文件文件目录定义好了以后，就可以开始编辑文件内容了。首先要编辑的是我们自定义的接口文件MyCarlaInterface.msg。我这里内容如下：string ego_idfloat64 ego_speed_xfloat64 ego_speed_yfloat64 ego_speed_zfloat64 ego_position_xfloat64 ego_position_yfloat64 ego_position_zstring obj_idfloat64 obj_speed_xfloat64 obj_speed_yfloat64 obj_speed_zfloat64 obj_position_xfloat64 obj_position_yfloat64 obj_position_z 这些都是我们在上一节文章中要发布和订阅的车辆信号，我这里统一把他们都定义在这个文件里面了。每个信号的格式都是“类型+变量名”，变量名是用户自己定义的，变量类型可以参考如下列表：boolbytecharfloat32float64int8uint8int16uint16int32uint32int64uint64string 注意，上面的这些类型都是最最最基础的类型，其实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 C99if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99)endif()# Default to C++14if(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 dependenciesfind_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_interfacesFinished <<< my_custom_interfaces [5.04s] Summary: 1 package finished [7.04s] 5、接口文件检查接下来我们使用ros2命令行工具检查下当前工作空间下的接口属性吧：source install/setup.bashros2 interface show my_custom_interfaces/msg/MyCarlaInterface 我们看到屏幕上打印出如下的内容：string ego_idfloat64 ego_speed_xfloat64 ego_speed_yfloat64 ego_speed_zfloat64 ego_position_xfloat64 ego_position_yfloat64 ego_position_zstring obj_idfloat64 obj_speed_xfloat64 obj_speed_yfloat64 obj_speed_zfloat64 obj_position_xfloat64 obj_position_yfloat64 obj_position_z 这可不就是我们定义好的接口文件的内容嘛，它现在已经在工作空间的接口库里了，后面我们就能直接调用了。6、创建新的功能包接下来就是对上一篇文章中的代码进行基于新接口的改造。切换到src目录下，创建新的功能包。cd srcros2 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.xml6 directories, 11 files 7、创建节点我们来继续定义aeb_control_pkg这个功能包下的节点文件，scenario_runner_node.py和controller_node.py。cd src/aeb_control_pkg/aeb_control_pkgtouch scenario_runner_node.pytouch controller_node.py 在scenario_runner_node.py中填入以下内容：import tracebackimport rclpyfrom rclpy.node import Nodefrom my_custom_interfaces.msg import MyCarlaInterfaceimport timeimport carlaimport tracebackclass 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 # passdef 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 rclpyfrom rclpy.node import Nodefrom my_custom_interfaces.msg import MyCarlaInterfaceimport carlaclass 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 edef 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 setuppackage_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.xml6 directories, 13 files 9、编译和试运行回到工作空间下，编译功能包：cd ~/ROS2_study/ros2_ws_demo006_custom_interface/colcon build 编译过程很快，我这里终端打印的消息长这样：Starting >>> aeb_control_pkgStarting >>> 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.bashros2 run aeb_control_pkg scenario_runner_node 此时，我们应该能从CARLA的服务端看到两辆车被创建了出来，并且主车逐渐靠近前方的目标车。如果没有下方的节点2启动，则主车会与目标车发生碰撞。然后在新打开一个终端，赶在碰撞前，把AEB控制节点运行起来：source install/setup.bashros2 run aeb_control_pkg controller_node 然后我们就能看到主车在撞到目标车前就制动停了下来。节点1打印的消息如下：节点2打印的消息如下：10、总结本节我们通过对车辆之间通信消息的自定义，把所有消息用一个接口文件包装了起来，这样在发布和订阅过程中都更加的优雅，逻辑更加清晰，代码简化了不少。然后基于新的接口文件重新优化了上一节的发布和订阅程序，实现了AEB功能的控制。完结，撒花。ROS 2中比较多使用这种自定义接口的方法，但是采用protobuf对数据进行发送能更轻量级、也能更定制化，但是我一直没有成功。不知道哪位道友有知道做法的可以一起探讨下。来源：车路漫漫
