在之前的文章里，我们在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_studymkdir ros2_ws_demo005_carla_aeb/cd ros2_ws_demo005_carla_aeb 3、创建功能包进入工作空间后，创建src文件夹来存放源码，在src下创建本次案例示例的功能包。mkdir srccd 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.py4 directories, 8 files 4、创建节点1-场景运行节点在'src/aeb_control_pkg/aeb_control_pkg'目录下新建一个python文件：scenario_runner_node.py，用作场景运行的节点，该文件的内容如下：import tracebackimport rclpyfrom rclpy.node import Nodefrom geometry_msgs.msg import PoseStampedfrom geometry_msgs.msg import TwistStampedfrom std_msgs.msg import Int32import 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.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 # 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() 5、创建节点2-AEB控制节点在'src/aeb_control_pkg/aeb_control_pkg'目录下新建一个python文件：controller_node.py，用作场景运行的节点，该文件的内容如下：import rclpyfrom rclpy.node import Nodefrom geometry_msgs.msg import PoseStampedfrom geometry_msgs.msg import TwistStampedfrom std_msgs.msg import Int32import 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.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 edef 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 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.py4 directories, 10 files 7、编译和试运行回到工作空间下，编译功能包：cd ~/ROS2_study/ros2_ws_demo005_carla_aebcolcon build 编译过程很快，我这里终端打印的消息长这样：Starting >>> aeb_control_pkgFinished <<< 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.bashros2 run aeb_control_pkg scenario_runner_node 此时，我们应该能从CARLA的服务端看到两辆车被创建了出来，并且主车逐渐靠近前方的目标车。如果没有下方的节点2启动，则主车会与目标车发生碰撞。然后再新打开一个终端，赶在碰撞前，把AEB控制节点运行起来：source install/setup.bashros2 run aeb_control_pkg controller_node 然后我们就能看到主车在撞到目标车前就制动停了下来。8、总结这篇文章，采用ROS 2，写了两个python节点，一个用来在CARLA中创建车辆使之行驶并将车辆信息以发布/订阅模式发送了出来，另一个节点订阅了车辆信息，并进行了TTC的计算，进而在碰撞前发出了控制指令，使得主车刹停了下来。这是一个非常简单的演示程序，在实际工作中，节点更多、发布订阅消息更丰富、控制程序更复杂，希望本篇能对大家的进一步探索提供一些思路。本文中对信号的发布及订阅非常笨重，下一节我打算用ROS 2的自定义接口来对本文代码进行升级。欢迎继续关注。来源：车路漫漫
