|
|
汽车零部件采购、销售通信录 填写你的培训需求，我们帮你找 招募汽车专业培训老师
在之前的文章里，我们在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的自定义接口来对本文代码进行升级。欢迎继续关注。 |
|
|手机版|小黑屋|Archiver|汽车工程师之家
( 渝ICP备18012993号-1 )
深度剖析 2019-2020 概念车设计趋势
智能座舱演进的思考
IP卡
狗仔卡
发表于 30-4-2024 19:39:45
提升卡
置顶卡
沉默卡
喧嚣卡
变色卡
显身卡