ROS2实战，我写了一个AEB程序，在CARLA里进行了仿真测试

babylemon828

在之前的文章里，我们在ROS2里完成了两个节点间的通信，怎么把这种通信方式应用到仿真测试的实际项目中呢。本文将进一步利用CARLA仿真，演示AEB算法的实现。
1、拟定AEB框架

简单的框架如下：


框架图
在上述的逻辑框图中，我们拟创建两个ROS 2节点，它们也都是CARLA的客户端。其中：

节点1的作用是运行场景，它在CARLA中创建一辆主车，在主车前方创建一辆目标车，主车以一定的初始速度行驶，撞向前方的目标车，这是最基础的场景设置，然后将车辆信息以发布者模式发送出去。

节点2的作用是AEB控制，它不断判断两辆车之间的TTC（Time To Collision，即碰撞时间），进而在合适的时候发出制动指令，控制主车制动。

在这个示例中，CARLA作为模拟器，提供了仿真环境。节点1在这个仿真环境中生成了主车和目标车，节点2将AEB算法的控制指令发送给主车，可以很方便地看到AEB激活后的控制效果。

敲定整体逻辑后，我们开始以代码来实现。
2、创建ROS 2工作空间

我们依然从最开始做起。

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

进入工作空间后，创建src文件夹来存放源码，在src下创建本次案例示例的功能包。
mkdir 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

4 directories, 8 files

4、创建节点1-场景运行节点

在'src/aeb_control_pkg/aeb_control_pkg'目录下新建一个python文件：scenario_runner_node.py，用作场景运行的节点，该文件的内容如下：
import traceback

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Int32
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.pose_publisher = self.create_publisher(PoseStamped, '/carla/pose/vehicle', 10)
        self.twist_publisher = self.create_publisher(TwistStamped, '/carla/twist/vehicle', 10)
        self.id_publisher = self.create_publisher(Int32, '/carla/ego/id', 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):
            pose_msg_ego = PoseStamped()
            pose_msg_ego.header.stamp = self.get_clock().now().to_msg()
            pose_msg_ego.header.frame_id = 'ego_car'
            pose_msg_ego.pose.position.x = self.ego_pos_x
            # pose_msg_ego.pose.position.x = 20.0
            pose_msg_ego.pose.position.y = 2.0
            pose_msg_ego.pose.position.z = 0.0
            pose_msg_obj = PoseStamped()
            pose_msg_obj.header.stamp = self.get_clock().now().to_msg()
            pose_msg_obj.header.frame_id = 'obj_car'
            pose_msg_obj.pose.position.x = self.obj_pos_x
            # pose_msg_obj.pose.position.x = 50.0
            pose_msg_obj.pose.position.y = 2.0
            pose_msg_obj.pose.position.z = 0.0

            twist_msg_ego = TwistStamped()
            twist_msg_ego.header.stamp = self.get_clock().now().to_msg()
            twist_msg_ego.header.frame_id = 'ego_car'
            twist_msg_ego.twist.linear.x = self.ego_speed_x  # 速度x
            # twist_msg_ego.twist.linear.x = 20.0
            twist_msg_ego.twist.linear.y = 0.0  # 速度y
            twist_msg_ego.twist.linear.z = 0.0  # 速度z
            twist_msg_obj = TwistStamped()
            twist_msg_obj.header.stamp = self.get_clock().now().to_msg()
            twist_msg_obj.header.frame_id = 'obj_car'
            twist_msg_obj.twist.linear.x = self.obj_speed_x  # 速度x
            # twist_msg_obj.twist.linear.x = 10.0
            twist_msg_obj.twist.linear.y = 0.0  # 速度y
            twist_msg_obj.twist.linear.z = 0.0  # 速度z

            ego_id = Int32()
            ego_id.data = self.ego_car.id

            # to pulish all message...
            self.pose_publisher.publish(pose_msg_ego)
            self.pose_publisher.publish(pose_msg_obj)
            self.twist_publisher.publish(twist_msg_ego)
            self.twist_publisher.publish(twist_msg_obj)
            self.id_publisher.publish(ego_id)
            # print("\npose_msg_ego:", pose_msg_ego)
            # print("\npose_msg_obj:", pose_msg_obj)
            # print("\ntwist_msg_ego:", twist_msg_ego)
            # print("\ntwist_msg_obj:", twist_msg_obj)

            self.logger.info('Publishing {} info: position_x={}, speed_x={}'.format(
                pose_msg_ego.header.frame_id,
                pose_msg_ego.pose.position.x,
                twist_msg_ego.twist.linear.x))
            self.logger.info('Publishing {} info: position_x={}, speed_x={}'.format(
                pose_msg_obj.header.frame_id,
                pose_msg_obj.pose.position.x,
                twist_msg_obj.twist.linear.x))
            self.logger.info('Publishing ego id:{}'.format(ego_id.data))
        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()

5、创建节点2-AEB控制节点

在'src/aeb_control_pkg/aeb_control_pkg'目录下新建一个python文件：controller_node.py，用作场景运行的节点，该文件的内容如下：
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Int32
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.pose_subscription = self.create_subscription(PoseStamped, '/carla/pose/vehicle', self.pose_callback, 10)
        self.twist_subscription = self.create_subscription(TwistStamped, '/carla/twist/vehicle', self.twist_callback, 10)
        self.id_subscription = self.create_subscription(Int32, '/carla/ego/id', self.id_callback, 10)
        #
        self.carla_world = None
        self.connect_to_carla()
        self.ego_car_id = None

    def id_callback(self, msg):
        if self.ego_car_id is None:
            self.ego_car_id = msg.data
        else:
            pass

    def pose_callback(self, msg):
        self.logger.info('Received: {}, pose_x={}'.format(msg.header.frame_id, msg.pose.position.x))
        if msg.header.frame_id == "ego_car":
            self.ego_position_x = msg.pose.position.x
        if msg.header.frame_id =="obj_car":
            self.obj_position_x = msg.pose.position.x

    def twist_callback(self, msg):
        self.logger.info('Received: {}, speed_x={}'.format(msg.header.frame_id, msg.twist.linear.x))
        if msg.header.frame_id == "ego_car":
            self.ego_speed_x = msg.twist.linear.x
        if msg.header.frame_id == "obj_car":
            self.obj_speed_x = msg.twist.linear.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()

6、配置节点

打开配置文件'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

4 directories, 10 files
7、编译和试运行

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

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

Summary: 1 package finished [4.29s]

这样就表示编译完成啦。

接下来就是测试了。

由于两个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

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



8、总结

这篇文章，采用ROS 2，写了两个python节点，一个用来在CARLA中创建车辆使之行驶并将车辆信息以发布/订阅模式发送了出来，另一个节点订阅了车辆信息，并进行了TTC的计算，进而在碰撞前发出了控制指令，使得主车刹停了下来。

这是一个非常简单的演示程序，在实际工作中，节点更多、发布订阅消息更丰富、控制程序更复杂，希望本篇能对大家的进一步探索提供一些思路。

本文中对信号的发布及订阅非常笨重，下一节我打算用ROS 2的自定义接口来对本文代码进行升级。欢迎继续关注。