暂时
点击查看代码
#!/usr/bin/env python3
"""
AirSim传感器数据ROS2发布器
功能：从AirSim获取RGB相机、深度相机和激光雷达数据，并发布到ROS2话题
作者：AI Assistant
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import airsim
import numpy as np
import cv2
from sensor_msgs.msg import Image, PointCloud2, LaserScan
from geometry_msgs.msg import PoseStamped, TransformStamped
from std_msgs.msg import Header
import sensor_msgs_py.point_cloud2 as pc2
from cv_bridge import CvBridge
from tf2_ros import TransformBroadcaster
import threading
import time
import logging
from typing import Optional, Dict, Any
import argparse
import yaml
import os
from scipy.spatial.transform import Rotation as R
class AirSimROS2Publisher(Node):
"""AirSim传感器数据ROS2发布器类"""
def __init__(self, config_file: str = None):
super().__init__('airsim_ros2_publisher')
# 配置日志
self.setup_logging()
# 加载配置
self.load_config(config_file)
# 初始化AirSim客户端
self.setup_airsim_client()
# 初始化ROS2发布器
self.setup_ros2_publishers()
# 初始化TF广播器
self.tf_broadcaster = TransformBroadcaster(self)
# 初始化CV Bridge
self.bridge = CvBridge()
# 控制变量
self.running = True
self.publish_threads = []
# 启动传感器数据发布线程
self.start_sensor_threads()
# 启动TF发布线程
if self.config['ros2']['publish_tf']:
self.start_tf_thread()
self.get_logger().info("AirSim ROS2 Publisher initialized successfully")
def setup_logging(self):
"""设置日志系统"""
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
handlers=[
logging.FileHandler('airsim_ros2_publisher.log'),
logging.StreamHandler()
]
)
self.logger = logging.getLogger(__name__)
def load_config(self, config_file: str):
"""加载配置文件"""
default_config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'ros2': {
'qos_reliability': 'RELIABLE', # RELIABLE or BEST_EFFORT
'qos_depth': 10,
'world_frame': 'world', # 世界坐标系
'publish_tf': True # 是否发布TF变换
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
if config_file and os.path.exists(config_file):
try:
with open(config_file, 'r') as f:
user_config = yaml.safe_load(f)
# 合并配置
self.config = self.merge_configs(default_config, user_config)
self.logger.info(f"Loaded configuration from {config_file}")
except Exception as e:
self.logger.error(f"Error loading config file: {e}")
self.config = default_config
else:
self.config = default_config
self.logger.info("Using default configuration")
def merge_configs(self, default: Dict, user: Dict) -> Dict:
"""合并配置字典"""
result = default.copy()
for key, value in user.items():
if key in result and isinstance(result[key], dict) and isinstance(value, dict):
result[key] = self.merge_configs(result[key], value)
else:
result[key] = value
return result
def setup_airsim_client(self):
"""初始化AirSim客户端"""
try:
# 为每个线程创建独立的客户端连接
self.airsim_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.airsim_client.confirmConnection()
self.airsim_client.enableApiControl(True)
self.vehicle_name = self.config['airsim']['vehicle_name']
self.logger.info(f"Connected to AirSim at {self.config['airsim']['host']}:{self.config['airsim']['port']}")
# 为每个传感器线程创建独立的客户端
self.rgb_client = None
self.depth_client = None
self.lidar_client = None
self.tf_client = None
except Exception as e:
self.logger.error(f"Failed to connect to AirSim: {e}")
raise
def setup_ros2_publishers(self):
"""初始化ROS2发布器"""
# 配置QoS - 根据配置选择策略
reliability = (ReliabilityPolicy.RELIABLE
if self.config['ros2']['qos_reliability'] == 'RELIABLE'
else ReliabilityPolicy.BEST_EFFORT)
qos_profile = QoSProfile(
reliability=reliability,
history=HistoryPolicy.KEEP_LAST,
depth=self.config['ros2']['qos_depth']
)
self.sensor_publishers = {}
# RGB相机发布器
if self.config['sensors']['rgb_camera']['enabled']:
self.sensor_publishers['rgb_camera'] = self.create_publisher(
Image,
self.config['sensors']['rgb_camera']['topic'],
qos_profile
)
self.logger.info(f"RGB camera publisher created on topic: {self.config['sensors']['rgb_camera']['topic']}")
# 深度相机发布器
if self.config['sensors']['depth_camera']['enabled']:
self.sensor_publishers['depth_camera'] = self.create_publisher(
Image,
self.config['sensors']['depth_camera']['topic'],
qos_profile
)
self.logger.info(f"Depth camera publisher created on topic: {self.config['sensors']['depth_camera']['topic']}")
# 激光雷达发布器
if self.config['sensors']['lidar']['enabled']:
self.sensor_publishers['lidar'] = self.create_publisher(
PointCloud2,
self.config['sensors']['lidar']['topic'],
qos_profile
)
self.logger.info(f"LiDAR publisher created on topic: {self.config['sensors']['lidar']['topic']}")
def get_rgb_image(self) -> Optional[np.ndarray]:
"""获取RGB相机图像"""
try:
# 为RGB线程创建独立客户端
if self.rgb_client is None:
self.rgb_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.rgb_client.confirmConnection()
camera_name = self.config['sensors']['rgb_camera']['camera_name']
response = self.rgb_client.simGetImage(
camera_name,
airsim.ImageType.Scene,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_rgb = cv2.imdecode(img1d, cv2.IMREAD_COLOR)
return cv2.cvtColor(img_rgb, cv2.COLOR_BGR2RGB)
except Exception as e:
self.logger.error(f"Error getting RGB image: {e}")
# 重置客户端以便下次重连
self.rgb_client = None
return None
def get_depth_image(self) -> Optional[np.ndarray]:
"""获取深度相机图像"""
try:
# 为深度相机线程创建独立客户端
if self.depth_client is None:
self.depth_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.depth_client.confirmConnection()
camera_name = self.config['sensors']['depth_camera']['camera_name']
response = self.depth_client.simGetImage(
camera_name,
airsim.ImageType.DepthPerspective,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_depth = cv2.imdecode(img1d, cv2.IMREAD_GRAYSCALE)
return img_depth
except Exception as e:
self.logger.error(f"Error getting depth image: {e}")
# 重置客户端以便下次重连
self.depth_client = None
return None
def get_vehicle_pose(self) -> Optional[Dict]:
"""获取无人机位置和姿态"""
try:
# 为TF线程创建独立客户端
if self.tf_client is None:
self.tf_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.tf_client.confirmConnection()
# 获取无人机状态
kinematics = self.tf_client.getMultirotorState(vehicle_name=self.vehicle_name).kinematics_estimated
# 位置 (NED坐标系)
position = kinematics.position
orientation = kinematics.orientation
return {
'position': {'x': position.x_val, 'y': position.y_val, 'z': position.z_val},
'orientation': {'x': orientation.x_val, 'y': orientation.y_val,
'z': orientation.z_val, 'w': orientation.w_val}
}
except Exception as e:
self.logger.error(f"Error getting vehicle pose: {e}")
# 重置客户端以便下次重连
self.tf_client = None
return None
def publish_transforms(self):
"""发布TF变换"""
rate = 30.0 # 30Hz
sleep_time = 1.0 / rate
while self.running:
try:
pose = self.get_vehicle_pose()
if pose is not None:
current_time = self.get_clock().now().to_msg()
# 发布 world -> vehicle_base_link 变换
t = TransformStamped()
t.header.stamp = current_time
t.header.frame_id = self.config['ros2']['world_frame']
t.child_frame_id = f"{self.vehicle_name}_base_link"
# 设置位置 (NED -> ENU坐标系转换)
t.transform.translation.x = pose['position']['y'] # NED的Y -> ENU的X
t.transform.translation.y = pose['position']['x'] # NED的X -> ENU的Y
t.transform.translation.z = -pose['position']['z'] # NED的Z -> ENU的Z
# 设置姿态 (NED -> ENU四元数转换)
# AirSim使用NED坐标系，ROS使用ENU坐标系
ned_quat = np.array([pose['orientation']['w'], pose['orientation']['x'],
pose['orientation']['y'], pose['orientation']['z']])
# NED到ENU的转换矩阵
ned_to_enu_rot = R.from_euler('xyz', [180, 0, 90], degrees=True)
vehicle_rot = R.from_quat([ned_quat[1], ned_quat[2], ned_quat[3], ned_quat[0]])
enu_rot = ned_to_enu_rot * vehicle_rot
enu_quat = enu_rot.as_quat()
t.transform.rotation.x = enu_quat[0]
t.transform.rotation.y = enu_quat[1]
t.transform.rotation.z = enu_quat[2]
t.transform.rotation.w = enu_quat[3]
self.tf_broadcaster.sendTransform(t)
# 发布传感器相对于base_link的静态变换
self.publish_sensor_transforms(current_time, f"{self.vehicle_name}_base_link")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in TF publishing thread: {e}")
time.sleep(1.0)
def publish_sensor_transforms(self, timestamp, parent_frame: str):
"""发布传感器相对于base_link的变换"""
# RGB相机变换
if self.config['sensors']['rgb_camera']['enabled']:
t_rgb = TransformStamped()
t_rgb.header.stamp = timestamp
t_rgb.header.frame_id = parent_frame
t_rgb.child_frame_id = self.config['sensors']['rgb_camera']['frame_id']
# 相机通常在前方
t_rgb.transform.translation.x = 0.1 # 前方10cm
t_rgb.transform.translation.y = 0.0
t_rgb.transform.translation.z = 0.0
t_rgb.transform.rotation.x = 0.0
t_rgb.transform.rotation.y = 0.0
t_rgb.transform.rotation.z = 0.0
t_rgb.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_rgb)
# 深度相机变换 (通常与RGB相机一致)
if self.config['sensors']['depth_camera']['enabled']:
t_depth = TransformStamped()
t_depth.header.stamp = timestamp
t_depth.header.frame_id = parent_frame
t_depth.child_frame_id = self.config['sensors']['depth_camera']['frame_id']
t_depth.transform.translation.x = 0.1
t_depth.transform.translation.y = 0.0
t_depth.transform.translation.z = 0.0
t_depth.transform.rotation.x = 0.0
t_depth.transform.rotation.y = 0.0
t_depth.transform.rotation.z = 0.0
t_depth.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_depth)
# 激光雷达变换
if self.config['sensors']['lidar']['enabled']:
t_lidar = TransformStamped()
t_lidar.header.stamp = timestamp
t_lidar.header.frame_id = parent_frame
t_lidar.child_frame_id = self.config['sensors']['lidar']['frame_id']
# 激光雷达通常在顶部
t_lidar.transform.translation.x = 0.0
t_lidar.transform.translation.y = 0.0
t_lidar.transform.translation.z = 0.1 # 顶部10cm
t_lidar.transform.rotation.x = 0.0
t_lidar.transform.rotation.y = 0.0
t_lidar.transform.rotation.z = 0.0
t_lidar.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_lidar)
def get_lidar_data(self) -> Optional[np.ndarray]:
"""获取激光雷达数据"""
try:
# 为激光雷达线程创建独立客户端
if self.lidar_client is None:
self.lidar_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.lidar_client.confirmConnection()
lidar_name = self.config['sensors']['lidar']['lidar_name']
lidar_data = self.lidar_client.getLidarData(
lidar_name,
vehicle_name=self.vehicle_name
)
# 修复LidarData属性访问
if len(lidar_data.point_cloud) > 0:
points = np.array(lidar_data.point_cloud).reshape(-1, 3)
self.logger.debug(f"LiDAR got {len(points)} points, shape: {points.shape}")
if len(points) > 0:
self.logger.debug(f"Point range - X: [{points[:, 0].min():.2f}, {points[:, 0].max():.2f}], "
f"Y: [{points[:, 1].min():.2f}, {points[:, 1].max():.2f}], "
f"Z: [{points[:, 2].min():.2f}, {points[:, 2].max():.2f}]")
return points
else:
self.logger.warning("LiDAR returned empty point cloud")
except Exception as e:
self.logger.error(f"Error getting LiDAR data: {e}")
# 重置客户端以便下次重连
self.lidar_client = None
return None
"""获取激光雷达数据"""
try:
lidar_name = self.config['sensors']['lidar']['lidar_name']
lidar_data = self.airsim_client.getLidarData(
lidar_name,
vehicle_name=self.vehicle_name
)
# 添加调试信息
if lidar_data.point_cloud_np.size > 0:
points = lidar_data.point_cloud_np.reshape(-1, 3)
self.logger.debug(f"LiDAR got {len(points)} points, shape: {points.shape}")
self.logger.debug(f"Point range - X: [{points[:, 0].min():.2f}, {points[:, 0].max():.2f}], "
f"Y: [{points[:, 1].min():.2f}, {points[:, 1].max():.2f}], "
f"Z: [{points[:, 2].min():.2f}, {points[:, 2].max():.2f}]")
return points
else:
self.logger.warning("LiDAR returned empty point cloud")
except Exception as e:
self.logger.error(f"Error getting LiDAR data: {e}")
return None
def publish_rgb_camera(self):
"""发布RGB相机数据"""
rate = self.config['sensors']['rgb_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
rgb_image = self.get_rgb_image()
if rgb_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['rgb_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(rgb_image, "rgb8")
ros_image.header = header
# 发布消息
self.sensor_publishers['rgb_camera'].publish(ros_image)
self.logger.debug("Published RGB camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in RGB camera publishing thread: {e}")
time.sleep(1.0)
def publish_depth_camera(self):
"""发布深度相机数据"""
rate = self.config['sensors']['depth_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
depth_image = self.get_depth_image()
if depth_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['depth_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(depth_image, "mono8")
ros_image.header = header
# 发布消息
self.sensor_publishers['depth_camera'].publish(ros_image)
self.logger.debug("Published depth camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in depth camera publishing thread: {e}")
time.sleep(1.0)
def publish_lidar(self):
"""发布激光雷达数据"""
rate = self.config['sensors']['lidar']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
lidar_points = self.get_lidar_data()
if lidar_points is not None and lidar_points.size > 0:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['lidar']['frame_id']
# 创建点云消息
point_cloud = pc2.create_cloud_xyz32(header, lidar_points)
# 发布消息
self.sensor_publishers['lidar'].publish(point_cloud)
self.logger.info(f"Published LiDAR point cloud with {len(lidar_points)} points")
else:
self.logger.warning("No LiDAR data to publish")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in LiDAR publishing thread: {e}")
time.sleep(1.0)
def start_sensor_threads(self):
"""启动传感器数据发布线程"""
if self.config['sensors']['rgb_camera']['enabled']:
rgb_thread = threading.Thread(target=self.publish_rgb_camera, daemon=True)
rgb_thread.start()
self.publish_threads.append(rgb_thread)
self.logger.info("RGB camera publishing thread started")
if self.config['sensors']['depth_camera']['enabled']:
depth_thread = threading.Thread(target=self.publish_depth_camera, daemon=True)
depth_thread.start()
self.publish_threads.append(depth_thread)
self.logger.info("Depth camera publishing thread started")
if self.config['sensors']['lidar']['enabled']:
lidar_thread = threading.Thread(target=self.publish_lidar, daemon=True)
lidar_thread.start()
self.publish_threads.append(lidar_thread)
self.logger.info("LiDAR publishing thread started")
def start_tf_thread(self):
"""启动TF发布线程"""
tf_thread = threading.Thread(target=self.publish_transforms, daemon=True)
tf_thread.start()
self.publish_threads.append(tf_thread)
self.logger.info("TF publishing thread started")
def shutdown(self):
"""关闭发布器"""
self.logger.info("Shutting down AirSim ROS2 Publisher...")
self.running = False
# 等待所有线程结束
for thread in self.publish_threads:
thread.join(timeout=2.0)
# 关闭AirSim连接
try:
if hasattr(self, 'airsim_client') and self.airsim_client:
self.airsim_client.enableApiControl(False)
if hasattr(self, 'rgb_client') and self.rgb_client:
self.rgb_client.enableApiControl(False)
if hasattr(self, 'depth_client') and self.depth_client:
self.depth_client.enableApiControl(False)
if hasattr(self, 'lidar_client') and self.lidar_client:
self.lidar_client.enableApiControl(False)
if hasattr(self, 'tf_client') and self.tf_client:
self.tf_client.enableApiControl(False)
except:
pass
self.logger.info("Shutdown complete")
def create_default_config():
"""创建默认配置文件"""
config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
with open('airsim_config.yaml', 'w') as f:
yaml.dump(config, f, default_flow_style=False)
print("Default configuration file created: airsim_config.yaml")
def main():
"""主函数"""
parser = argparse.ArgumentParser(description='AirSim ROS2 Publisher')
parser.add_argument('--config', type=str, help='Configuration file path')
parser.add_argument('--create-config', action='store_true', help='Create default configuration file')
args = parser.parse_args()
if args.create_config:
create_default_config()
return
# 初始化ROS2
rclpy.init()
try:
# 创建发布器节点
publisher = AirSimROS2Publisher(args.config)
# 运行节点
rclpy.spin(publisher)
except KeyboardInterrupt:
print("\nShutdown requested by user")
except Exception as e:
print(f"Error: {e}")
finally:
# 清理
if 'publisher' in locals():
publisher.shutdown()
rclpy.shutdown()
if __name__ == '__main__':
main()
点击查看代码
#!/usr/bin/env python3
"""
AirSim传感器数据ROS2发布器
功能：从AirSim获取RGB相机、深度相机和激光雷达数据，并发布到ROS2话题
作者：AI Assistant
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import airsim
import numpy as np
import cv2
from sensor_msgs.msg import Image, PointCloud2, LaserScan
from geometry_msgs.msg import PoseStamped, TransformStamped
from std_msgs.msg import Header
import sensor_msgs_py.point_cloud2 as pc2
from cv_bridge import CvBridge
from tf2_ros import TransformBroadcaster
import threading
import time
import logging
from typing import Optional, Dict, Any
import argparse
import yaml
import os
from scipy.spatial.transform import Rotation as R
class AirSimROS2Publisher(Node):
"""AirSim传感器数据ROS2发布器类"""
def __init__(self, config_file: str = None):
super().__init__('airsim_ros2_publisher')
# 配置日志
self.setup_logging()
# 加载配置
self.load_config(config_file)
# 初始化AirSim客户端
self.setup_airsim_client()
# 初始化ROS2发布器
self.setup_ros2_publishers()
# 初始化TF广播器
self.tf_broadcaster = TransformBroadcaster(self)
# 初始化CV Bridge
self.bridge = CvBridge()
# 控制变量
self.running = True
self.publish_threads = []
# 启动传感器数据发布线程
self.start_sensor_threads()
# 启动TF发布线程
if self.config['ros2']['publish_tf']:
self.start_tf_thread()
self.get_logger().info("AirSim ROS2 Publisher initialized successfully")
def setup_logging(self):
"""设置日志系统"""
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
handlers=[
logging.FileHandler('airsim_ros2_publisher.log'),
logging.StreamHandler()
]
)
self.logger = logging.getLogger(__name__)
def load_config(self, config_file: str):
"""加载配置文件"""
default_config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'ros2': {
'qos_reliability': 'RELIABLE', # RELIABLE or BEST_EFFORT
'qos_depth': 10,
'world_frame': 'world', # 世界坐标系
'publish_tf': True # 是否发布TF变换
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
if config_file and os.path.exists(config_file):
try:
with open(config_file, 'r') as f:
user_config = yaml.safe_load(f)
# 合并配置
self.config = self.merge_configs(default_config, user_config)
self.logger.info(f"Loaded configuration from {config_file}")
except Exception as e:
self.logger.error(f"Error loading config file: {e}")
self.config = default_config
else:
self.config = default_config
self.logger.info("Using default configuration")
def merge_configs(self, default: Dict, user: Dict) -> Dict:
"""合并配置字典"""
result = default.copy()
for key, value in user.items():
if key in result and isinstance(result[key], dict) and isinstance(value, dict):
result[key] = self.merge_configs(result[key], value)
else:
result[key] = value
return result
def setup_airsim_client(self):
"""初始化AirSim客户端"""
try:
self.airsim_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.airsim_client.confirmConnection()
self.airsim_client.enableApiControl(True)
self.vehicle_name = self.config['airsim']['vehicle_name']
self.logger.info(f"Connected to AirSim at {self.config['airsim']['host']}:{self.config['airsim']['port']}")
except Exception as e:
self.logger.error(f"Failed to connect to AirSim: {e}")
raise
def setup_ros2_publishers(self):
"""初始化ROS2发布器"""
# 配置QoS - 根据配置选择策略
reliability = (ReliabilityPolicy.RELIABLE
if self.config['ros2']['qos_reliability'] == 'RELIABLE'
else ReliabilityPolicy.BEST_EFFORT)
qos_profile = QoSProfile(
reliability=reliability,
history=HistoryPolicy.KEEP_LAST,
depth=self.config['ros2']['qos_depth']
)
self.publishers = {}
# RGB相机发布器
if self.config['sensors']['rgb_camera']['enabled']:
self.publishers['rgb_camera'] = self.create_publisher(
Image,
self.config['sensors']['rgb_camera']['topic'],
qos_profile
)
self.logger.info(f"RGB camera publisher created on topic: {self.config['sensors']['rgb_camera']['topic']}")
# 深度相机发布器
if self.config['sensors']['depth_camera']['enabled']:
self.publishers['depth_camera'] = self.create_publisher(
Image,
self.config['sensors']['depth_camera']['topic'],
qos_profile
)
self.logger.info(f"Depth camera publisher created on topic: {self.config['sensors']['depth_camera']['topic']}")
# 激光雷达发布器
if self.config['sensors']['lidar']['enabled']:
self.publishers['lidar'] = self.create_publisher(
PointCloud2,
self.config['sensors']['lidar']['topic'],
qos_profile
)
self.logger.info(f"LiDAR publisher created on topic: {self.config['sensors']['lidar']['topic']}")
def get_rgb_image(self) -> Optional[np.ndarray]:
"""获取RGB相机图像"""
try:
camera_name = self.config['sensors']['rgb_camera']['camera_name']
response = self.airsim_client.simGetImage(
camera_name,
airsim.ImageType.Scene,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_rgb = cv2.imdecode(img1d, cv2.IMREAD_COLOR)
return cv2.cvtColor(img_rgb, cv2.COLOR_BGR2RGB)
except Exception as e:
self.logger.error(f"Error getting RGB image: {e}")
return None
def get_depth_image(self) -> Optional[np.ndarray]:
"""获取深度相机图像"""
try:
camera_name = self.config['sensors']['depth_camera']['camera_name']
response = self.airsim_client.simGetImage(
camera_name,
airsim.ImageType.DepthPerspective,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_depth = cv2.imdecode(img1d, cv2.IMREAD_GRAYSCALE)
return img_depth
except Exception as e:
self.logger.error(f"Error getting depth image: {e}")
return None
def get_vehicle_pose(self) -> Optional[Dict]:
"""获取无人机位置和姿态"""
try:
# 获取无人机状态
kinematics = self.airsim_client.getMultirotorState(vehicle_name=self.vehicle_name).kinematics_estimated
# 位置 (NED坐标系)
position = kinematics.position
orientation = kinematics.orientation
return {
'position': {'x': position.x_val, 'y': position.y_val, 'z': position.z_val},
'orientation': {'x': orientation.x_val, 'y': orientation.y_val,
'z': orientation.z_val, 'w': orientation.w_val}
}
except Exception as e:
self.logger.error(f"Error getting vehicle pose: {e}")
return None
def publish_transforms(self):
"""发布TF变换"""
rate = 30.0 # 30Hz
sleep_time = 1.0 / rate
while self.running:
try:
pose = self.get_vehicle_pose()
if pose is not None:
current_time = self.get_clock().now().to_msg()
# 发布 world -> vehicle_base_link 变换
t = TransformStamped()
t.header.stamp = current_time
t.header.frame_id = self.config['ros2']['world_frame']
t.child_frame_id = f"{self.vehicle_name}_base_link"
# 设置位置 (NED -> ENU坐标系转换)
t.transform.translation.x = pose['position']['y'] # NED的Y -> ENU的X
t.transform.translation.y = pose['position']['x'] # NED的X -> ENU的Y
t.transform.translation.z = -pose['position']['z'] # NED的Z -> ENU的Z
# 设置姿态 (NED -> ENU四元数转换)
# AirSim使用NED坐标系，ROS使用ENU坐标系
ned_quat = np.array([pose['orientation']['w'], pose['orientation']['x'],
pose['orientation']['y'], pose['orientation']['z']])
# NED到ENU的转换矩阵
ned_to_enu_rot = R.from_euler('xyz', [180, 0, 90], degrees=True)
vehicle_rot = R.from_quat([ned_quat[1], ned_quat[2], ned_quat[3], ned_quat[0]])
enu_rot = ned_to_enu_rot * vehicle_rot
enu_quat = enu_rot.as_quat()
t.transform.rotation.x = enu_quat[0]
t.transform.rotation.y = enu_quat[1]
t.transform.rotation.z = enu_quat[2]
t.transform.rotation.w = enu_quat[3]
self.tf_broadcaster.sendTransform(t)
# 发布传感器相对于base_link的静态变换
self.publish_sensor_transforms(current_time, f"{self.vehicle_name}_base_link")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in TF publishing thread: {e}")
time.sleep(1.0)
def publish_sensor_transforms(self, timestamp, parent_frame: str):
"""发布传感器相对于base_link的变换"""
# RGB相机变换
if self.config['sensors']['rgb_camera']['enabled']:
t_rgb = TransformStamped()
t_rgb.header.stamp = timestamp
t_rgb.header.frame_id = parent_frame
t_rgb.child_frame_id = self.config['sensors']['rgb_camera']['frame_id']
# 相机通常在前方
t_rgb.transform.translation.x = 0.1 # 前方10cm
t_rgb.transform.translation.y = 0.0
t_rgb.transform.translation.z = 0.0
t_rgb.transform.rotation.x = 0.0
t_rgb.transform.rotation.y = 0.0
t_rgb.transform.rotation.z = 0.0
t_rgb.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_rgb)
# 深度相机变换 (通常与RGB相机一致)
if self.config['sensors']['depth_camera']['enabled']:
t_depth = TransformStamped()
t_depth.header.stamp = timestamp
t_depth.header.frame_id = parent_frame
t_depth.child_frame_id = self.config['sensors']['depth_camera']['frame_id']
t_depth.transform.translation.x = 0.1
t_depth.transform.translation.y = 0.0
t_depth.transform.translation.z = 0.0
t_depth.transform.rotation.x = 0.0
t_depth.transform.rotation.y = 0.0
t_depth.transform.rotation.z = 0.0
t_depth.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_depth)
# 激光雷达变换
if self.config['sensors']['lidar']['enabled']:
t_lidar = TransformStamped()
t_lidar.header.stamp = timestamp
t_lidar.header.frame_id = parent_frame
t_lidar.child_frame_id = self.config['sensors']['lidar']['frame_id']
# 激光雷达通常在顶部
t_lidar.transform.translation.x = 0.0
t_lidar.transform.translation.y = 0.0
t_lidar.transform.translation.z = 0.1 # 顶部10cm
t_lidar.transform.rotation.x = 0.0
t_lidar.transform.rotation.y = 0.0
t_lidar.transform.rotation.z = 0.0
t_lidar.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t_lidar)
"""获取激光雷达数据"""
try:
lidar_name = self.config['sensors']['lidar']['lidar_name']
lidar_data = self.airsim_client.getLidarData(
lidar_name,
vehicle_name=self.vehicle_name
)
# 添加调试信息
if lidar_data.point_cloud_np.size > 0:
points = lidar_data.point_cloud_np.reshape(-1, 3)
self.logger.debug(f"LiDAR got {len(points)} points, shape: {points.shape}")
self.logger.debug(f"Point range - X: [{points[:, 0].min():.2f}, {points[:, 0].max():.2f}], "
f"Y: [{points[:, 1].min():.2f}, {points[:, 1].max():.2f}], "
f"Z: [{points[:, 2].min():.2f}, {points[:, 2].max():.2f}]")
return points
else:
self.logger.warning("LiDAR returned empty point cloud")
except Exception as e:
self.logger.error(f"Error getting LiDAR data: {e}")
return None
def publish_rgb_camera(self):
"""发布RGB相机数据"""
rate = self.config['sensors']['rgb_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
rgb_image = self.get_rgb_image()
if rgb_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['rgb_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(rgb_image, "rgb8")
ros_image.header = header
# 发布消息
self.publishers['rgb_camera'].publish(ros_image)
self.logger.debug("Published RGB camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in RGB camera publishing thread: {e}")
time.sleep(1.0)
def publish_depth_camera(self):
"""发布深度相机数据"""
rate = self.config['sensors']['depth_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
depth_image = self.get_depth_image()
if depth_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['depth_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(depth_image, "mono8")
ros_image.header = header
# 发布消息
self.publishers['depth_camera'].publish(ros_image)
self.logger.debug("Published depth camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in depth camera publishing thread: {e}")
time.sleep(1.0)
def publish_lidar(self):
"""发布激光雷达数据"""
rate = self.config['sensors']['lidar']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
lidar_points = self.get_lidar_data()
if lidar_points is not None and lidar_points.size > 0:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['lidar']['frame_id']
# 创建点云消息
point_cloud = pc2.create_cloud_xyz32(header, lidar_points)
# 发布消息
self.publishers['lidar'].publish(point_cloud)
self.logger.info(f"Published LiDAR point cloud with {len(lidar_points)} points")
else:
self.logger.warning("No LiDAR data to publish")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in LiDAR publishing thread: {e}")
time.sleep(1.0)
def start_sensor_threads(self):
"""启动传感器数据发布线程"""
if self.config['sensors']['rgb_camera']['enabled']:
rgb_thread = threading.Thread(target=self.publish_rgb_camera, daemon=True)
rgb_thread.start()
self.publish_threads.append(rgb_thread)
self.logger.info("RGB camera publishing thread started")
if self.config['sensors']['depth_camera']['enabled']:
depth_thread = threading.Thread(target=self.publish_depth_camera, daemon=True)
depth_thread.start()
self.publish_threads.append(depth_thread)
self.logger.info("Depth camera publishing thread started")
if self.config['sensors']['lidar']['enabled']:
lidar_thread = threading.Thread(target=self.publish_lidar, daemon=True)
lidar_thread.start()
self.publish_threads.append(lidar_thread)
self.logger.info("LiDAR publishing thread started")
def start_tf_thread(self):
"""启动TF发布线程"""
tf_thread = threading.Thread(target=self.publish_transforms, daemon=True)
tf_thread.start()
self.publish_threads.append(tf_thread)
self.logger.info("TF publishing thread started")
def shutdown(self):
"""关闭发布器"""
self.logger.info("Shutting down AirSim ROS2 Publisher...")
self.running = False
# 等待所有线程结束
for thread in self.publish_threads:
thread.join(timeout=2.0)
# 关闭AirSim连接
try:
self.airsim_client.enableApiControl(False)
except:
pass
self.logger.info("Shutdown complete")
def create_default_config():
"""创建默认配置文件"""
config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
with open('airsim_config.yaml', 'w') as f:
yaml.dump(config, f, default_flow_style=False)
print("Default configuration file created: airsim_config.yaml")
def main():
"""主函数"""
parser = argparse.ArgumentParser(description='AirSim ROS2 Publisher')
parser.add_argument('--config', type=str, help='Configuration file path')
parser.add_argument('--create-config', action='store_true', help='Create default configuration file')
args = parser.parse_args()
if args.create_config:
create_default_config()
return
# 初始化ROS2
rclpy.init()
try:
# 创建发布器节点
publisher = AirSimROS2Publisher(args.config)
# 运行节点
rclpy.spin(publisher)
except KeyboardInterrupt:
print("\nShutdown requested by user")
except Exception as e:
print(f"Error: {e}")
finally:
# 清理
if 'publisher' in locals():
publisher.shutdown()
rclpy.shutdown()
if __name__ == '__main__':
main()
点击查看代码
#!/usr/bin/env python3
"""
AirSim传感器数据ROS2发布器
功能：从AirSim获取RGB相机、深度相机和激光雷达数据，并发布到ROS2话题
支持点云地图累积和坐标变换发布
作者：AI Assistant
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import airsim
import numpy as np
import cv2
from sensor_msgs.msg import Image, PointCloud2, LaserScan
from geometry_msgs.msg import PoseStamped, TransformStamped
from std_msgs.msg import Header
import sensor_msgs_py.point_cloud2 as pc2
from cv_bridge import CvBridge
from tf2_ros import TransformBroadcaster
import threading
import time
import logging
from typing import Optional, Dict, Any
import argparse
import yaml
import os
from scipy.spatial.transform import Rotation as R
class AirSimROS2Publisher(Node):
"""AirSim传感器数据ROS2发布器类"""
def __init__(self, config_file: str = None):
super().__init__('airsim_ros2_publisher')
# 配置日志
self.setup_logging()
# 加载配置
self.load_config(config_file)
# 初始化AirSim客户端
self.setup_airsim_client()
# 初始化ROS2发布器
self.setup_ros2_publishers()
# 初始化CV Bridge
self.bridge = CvBridge()
# 初始化TF广播器
self.tf_broadcaster = TransformBroadcaster(self)
# 点云累积相关
self.accumulated_points = []
self.max_accumulated_points = self.config.get('mapping', {}).get('max_points', 1000000)
self.accumulation_enabled = self.config.get('mapping', {}).get('enable_accumulation', True)
# 控制变量
self.running = True
self.publish_threads = []
# 启动传感器数据发布线程
self.start_sensor_threads()
self.get_logger().info("AirSim ROS2 Publisher initialized successfully")
def setup_logging(self):
"""设置日志系统"""
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
handlers=[
logging.FileHandler('airsim_ros2_publisher.log'),
logging.StreamHandler()
]
)
self.logger = logging.getLogger(__name__)
def load_config(self, config_file: str):
"""加载配置文件"""
default_config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'ros2': {
'qos_reliability': 'RELIABLE', # RELIABLE or BEST_EFFORT
'qos_depth': 10
},
'mapping': {
'enable_accumulation': True, # 是否启用点云累积
'max_points': 1000000, # 最大累积点数
'world_frame': 'world', # 世界坐标系
'publish_tf': True # 是否发布坐标变换
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'accumulated_topic': '/airsim/lidar/map', # 累积点云话题
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
if config_file and os.path.exists(config_file):
try:
with open(config_file, 'r') as f:
user_config = yaml.safe_load(f)
# 合并配置
self.config = self.merge_configs(default_config, user_config)
self.logger.info(f"Loaded configuration from {config_file}")
except Exception as e:
self.logger.error(f"Error loading config file: {e}")
self.config = default_config
else:
self.config = default_config
self.logger.info("Using default configuration")
def merge_configs(self, default: Dict, user: Dict) -> Dict:
"""合并配置字典"""
result = default.copy()
for key, value in user.items():
if key in result and isinstance(result[key], dict) and isinstance(value, dict):
result[key] = self.merge_configs(result[key], value)
else:
result[key] = value
return result
def setup_airsim_client(self):
"""初始化AirSim客户端"""
try:
self.airsim_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.airsim_client.confirmConnection()
self.airsim_client.enableApiControl(True)
self.vehicle_name = self.config['airsim']['vehicle_name']
self.logger.info(f"Connected to AirSim at {self.config['airsim']['host']}:{self.config['airsim']['port']}")
except Exception as e:
self.logger.error(f"Failed to connect to AirSim: {e}")
raise
def setup_ros2_publishers(self):
"""初始化ROS2发布器"""
# 配置QoS - 根据配置选择策略
reliability = (ReliabilityPolicy.RELIABLE
if self.config['ros2']['qos_reliability'] == 'RELIABLE'
else ReliabilityPolicy.BEST_EFFORT)
qos_profile = QoSProfile(
reliability=reliability,
history=HistoryPolicy.KEEP_LAST,
depth=self.config['ros2']['qos_depth']
)
self.publishers = {}
# RGB相机发布器
if self.config['sensors']['rgb_camera']['enabled']:
self.publishers['rgb_camera'] = self.create_publisher(
Image,
self.config['sensors']['rgb_camera']['topic'],
qos_profile
)
self.logger.info(f"RGB camera publisher created on topic: {self.config['sensors']['rgb_camera']['topic']}")
# 深度相机发布器
if self.config['sensors']['depth_camera']['enabled']:
self.publishers['depth_camera'] = self.create_publisher(
Image,
self.config['sensors']['depth_camera']['topic'],
qos_profile
)
self.logger.info(f"Depth camera publisher created on topic: {self.config['sensors']['depth_camera']['topic']}")
# 激光雷达发布器
if self.config['sensors']['lidar']['enabled']:
self.publishers['lidar'] = self.create_publisher(
PointCloud2,
self.config['sensors']['lidar']['topic'],
qos_profile
)
self.logger.info(f"LiDAR publisher created on topic: {self.config['sensors']['lidar']['topic']}")
# 累积点云发布器
if self.accumulation_enabled:
self.publishers['lidar_map'] = self.create_publisher(
PointCloud2,
self.config['sensors']['lidar']['accumulated_topic'],
qos_profile
)
self.logger.info(f"LiDAR map publisher created on topic: {self.config['sensors']['lidar']['accumulated_topic']}")
def get_rgb_image(self) -> Optional[np.ndarray]:
"""获取RGB相机图像"""
try:
camera_name = self.config['sensors']['rgb_camera']['camera_name']
response = self.airsim_client.simGetImage(
camera_name,
airsim.ImageType.Scene,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_rgb = cv2.imdecode(img1d, cv2.IMREAD_COLOR)
return cv2.cvtColor(img_rgb, cv2.COLOR_BGR2RGB)
except Exception as e:
self.logger.error(f"Error getting RGB image: {e}")
return None
def get_depth_image(self) -> Optional[np.ndarray]:
"""获取深度相机图像"""
try:
camera_name = self.config['sensors']['depth_camera']['camera_name']
response = self.airsim_client.simGetImage(
camera_name,
airsim.ImageType.DepthPerspective,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_depth = cv2.imdecode(img1d, cv2.IMREAD_GRAYSCALE)
return img_depth
except Exception as e:
self.logger.error(f"Error getting depth image: {e}")
return None
def get_vehicle_pose(self):
"""获取无人机位姿"""
try:
pose = self.airsim_client.simGetVehiclePose(vehicle_name=self.vehicle_name)
return pose
except Exception as e:
self.logger.error(f"Error getting vehicle pose: {e}")
return None
def publish_tf(self, pose):
"""发布坐标变换"""
if not self.config.get('mapping', {}).get('publish_tf', True):
return
try:
transform = TransformStamped()
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = self.config['mapping']['world_frame']
transform.child_frame_id = self.vehicle_name + '_base_link'
# 位置
transform.transform.translation.x = pose.position.x_val
transform.transform.translation.y = pose.position.y_val
transform.transform.translation.z = pose.position.z_val
# 姿态
transform.transform.rotation.w = pose.orientation.w_val
transform.transform.rotation.x = pose.orientation.x_val
transform.transform.rotation.y = pose.orientation.y_val
transform.transform.rotation.z = pose.orientation.z_val
self.tf_broadcaster.sendTransform(transform)
# 发布激光雷达到base_link的变换
lidar_transform = TransformStamped()
lidar_transform.header.stamp = transform.header.stamp
lidar_transform.header.frame_id = self.vehicle_name + '_base_link'
lidar_transform.child_frame_id = self.config['sensors']['lidar']['frame_id']
# 激光雷达相对于无人机的位置（根据实际情况调整）
lidar_transform.transform.translation.x = 0.0
lidar_transform.transform.translation.y = 0.0
lidar_transform.transform.translation.z = 0.0
lidar_transform.transform.rotation.w = 1.0
lidar_transform.transform.rotation.x = 0.0
lidar_transform.transform.rotation.y = 0.0
lidar_transform.transform.rotation.z = 0.0
self.tf_broadcaster.sendTransform(lidar_transform)
except Exception as e:
self.logger.error(f"Error publishing TF: {e}")
def transform_points_to_world(self, points, pose):
"""将点云从激光雷达坐标系转换到世界坐标系"""
try:
# AirSim的姿态是四元数，转换为旋转矩阵
quat = [pose.orientation.x_val, pose.orientation.y_val,
pose.orientation.z_val, pose.orientation.w_val]
rotation = R.from_quat(quat)
rotation_matrix = rotation.as_matrix()
# 平移向量
translation = np.array([pose.position.x_val, pose.position.y_val, pose.position.z_val])
# 应用变换
world_points = np.dot(points, rotation_matrix.T) + translation
return world_points
except Exception as e:
self.logger.error(f"Error transforming points: {e}")
return points
"""获取激光雷达数据"""
try:
lidar_name = self.config['sensors']['lidar']['lidar_name']
lidar_data = self.airsim_client.getLidarData(
lidar_name,
vehicle_name=self.vehicle_name
)
# 添加调试信息
if lidar_data.point_cloud_np.size > 0:
points = lidar_data.point_cloud_np.reshape(-1, 3)
self.logger.debug(f"LiDAR got {len(points)} points, shape: {points.shape}")
self.logger.debug(f"Point range - X: [{points[:, 0].min():.2f}, {points[:, 0].max():.2f}], "
f"Y: [{points[:, 1].min():.2f}, {points[:, 1].max():.2f}], "
f"Z: [{points[:, 2].min():.2f}, {points[:, 2].max():.2f}]")
return points
else:
self.logger.warning("LiDAR returned empty point cloud")
except Exception as e:
self.logger.error(f"Error getting LiDAR data: {e}")
return None
def publish_rgb_camera(self):
"""发布RGB相机数据"""
rate = self.config['sensors']['rgb_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
rgb_image = self.get_rgb_image()
if rgb_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['rgb_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(rgb_image, "rgb8")
ros_image.header = header
# 发布消息
self.publishers['rgb_camera'].publish(ros_image)
self.logger.debug("Published RGB camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in RGB camera publishing thread: {e}")
time.sleep(1.0)
def publish_depth_camera(self):
"""发布深度相机数据"""
rate = self.config['sensors']['depth_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
depth_image = self.get_depth_image()
if depth_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['depth_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(depth_image, "mono8")
ros_image.header = header
# 发布消息
self.publishers['depth_camera'].publish(ros_image)
self.logger.debug("Published depth camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in depth camera publishing thread: {e}")
time.sleep(1.0)
def publish_lidar(self):
"""发布激光雷达数据"""
rate = self.config['sensors']['lidar']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
# 获取无人机位姿
pose = self.get_vehicle_pose()
if pose is None:
time.sleep(sleep_time)
continue
# 发布坐标变换
self.publish_tf(pose)
# 获取激光雷达数据
lidar_points = self.get_lidar_data()
if lidar_points is not None and lidar_points.size > 0:
current_time = self.get_clock().now().to_msg()
# 发布实时点云（相对于激光雷达坐标系）
header = Header()
header.stamp = current_time
header.frame_id = self.config['sensors']['lidar']['frame_id']
point_cloud = pc2.create_cloud_xyz32(header, lidar_points)
self.publishers['lidar'].publish(point_cloud)
# 处理累积点云
if self.accumulation_enabled:
# 转换到世界坐标系
world_points = self.transform_points_to_world(lidar_points, pose)
# 累积点云
self.accumulated_points.extend(world_points.tolist())
# 限制点云数量
if len(self.accumulated_points) > self.max_accumulated_points:
# 删除最老的点
excess = len(self.accumulated_points) - self.max_accumulated_points
self.accumulated_points = self.accumulated_points[excess:]
# 发布累积点云地图
if len(self.accumulated_points) > 0:
map_header = Header()
map_header.stamp = current_time
map_header.frame_id = self.config['mapping']['world_frame']
accumulated_array = np.array(self.accumulated_points)
map_cloud = pc2.create_cloud_xyz32(map_header, accumulated_array)
self.publishers['lidar_map'].publish(map_cloud)
self.logger.info(f"Published LiDAR: {len(lidar_points)} current points, "
f"{len(self.accumulated_points)} accumulated points")
else:
self.logger.debug(f"Published LiDAR point cloud with {len(lidar_points)} points")
else:
self.logger.warning("No LiDAR data to publish")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in LiDAR publishing thread: {e}")
time.sleep(1.0)
def start_sensor_threads(self):
"""启动传感器数据发布线程"""
if self.config['sensors']['rgb_camera']['enabled']:
rgb_thread = threading.Thread(target=self.publish_rgb_camera, daemon=True)
rgb_thread.start()
self.publish_threads.append(rgb_thread)
self.logger.info("RGB camera publishing thread started")
if self.config['sensors']['depth_camera']['enabled']:
depth_thread = threading.Thread(target=self.publish_depth_camera, daemon=True)
depth_thread.start()
self.publish_threads.append(depth_thread)
self.logger.info("Depth camera publishing thread started")
if self.config['sensors']['lidar']['enabled']:
lidar_thread = threading.Thread(target=self.publish_lidar, daemon=True)
lidar_thread.start()
self.publish_threads.append(lidar_thread)
self.logger.info("LiDAR publishing thread started")
def shutdown(self):
"""关闭发布器"""
self.logger.info("Shutting down AirSim ROS2 Publisher...")
self.running = False
# 等待所有线程结束
for thread in self.publish_threads:
thread.join(timeout=2.0)
# 关闭AirSim连接
try:
self.airsim_client.enableApiControl(False)
except:
pass
self.logger.info("Shutdown complete")
def create_default_config():
"""创建默认配置文件"""
config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
with open('airsim_config.yaml', 'w') as f:
yaml.dump(config, f, default_flow_style=False)
print("Default configuration file created: airsim_config.yaml")
def main():
"""主函数"""
parser = argparse.ArgumentParser(description='AirSim ROS2 Publisher')
parser.add_argument('--config', type=str, help='Configuration file path')
parser.add_argument('--create-config', action='store_true', help='Create default configuration file')
args = parser.parse_args()
if args.create_config:
create_default_config()
return
# 初始化ROS2
rclpy.init()
try:
# 创建发布器节点
publisher = AirSimROS2Publisher(args.config)
# 运行节点
rclpy.spin(publisher)
except KeyboardInterrupt:
print("\nShutdown requested by user")
except Exception as e:
print(f"Error: {e}")
finally:
# 清理
if 'publisher' in locals():
publisher.shutdown()
rclpy.shutdown()
if __name__ == '__main__':
main()
python
#!/usr/bin/env python3
"""
AirSim传感器数据ROS2发布器
功能：从AirSim获取RGB相机、深度相机和激光雷达数据，并发布到ROS2话题
作者：AI Assistant
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import airsim
import numpy as np
import cv2
from sensor_msgs.msg import Image, PointCloud2, LaserScan
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
import sensor_msgs_py.point_cloud2 as pc2
from cv_bridge import CvBridge
import threading
import time
import logging
from typing import Optional, Dict, Any
import argparse
import yaml
import os
class AirSimROS2Publisher(Node):
"""AirSim传感器数据ROS2发布器类"""
def __init__(self, config_file: str = None):
super().__init__('airsim_ros2_publisher')
# 配置日志
self.setup_logging()
# 加载配置
self.load_config(config_file)
# 初始化AirSim客户端
self.setup_airsim_client()
# 初始化ROS2发布器
self.setup_ros2_publishers()
# 初始化CV Bridge
self.bridge = CvBridge()
# 控制变量
self.running = True
self.publish_threads = []
# 启动传感器数据发布线程
self.start_sensor_threads()
self.get_logger().info("AirSim ROS2 Publisher initialized successfully")
def setup_logging(self):
"""设置日志系统"""
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
handlers=[
logging.FileHandler('airsim_ros2_publisher.log'),
logging.StreamHandler()
]
)
self.logger = logging.getLogger(__name__)
def load_config(self, config_file: str):
"""加载配置文件"""
default_config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
if config_file and os.path.exists(config_file):
try:
with open(config_file, 'r') as f:
user_config = yaml.safe_load(f)
# 合并配置
self.config = self.merge_configs(default_config, user_config)
self.logger.info(f"Loaded configuration from {config_file}")
except Exception as e:
self.logger.error(f"Error loading config file: {e}")
self.config = default_config
else:
self.config = default_config
self.logger.info("Using default configuration")
def merge_configs(self, default: Dict, user: Dict) -> Dict:
"""合并配置字典"""
result = default.copy()
for key, value in user.items():
if key in result and isinstance(result[key], dict) and isinstance(value, dict):
result[key] = self.merge_configs(result[key], value)
else:
result[key] = value
return result
def setup_airsim_client(self):
"""初始化AirSim客户端"""
try:
self.airsim_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.airsim_client.confirmConnection()
self.airsim_client.enableApiControl(True)
self.vehicle_name = self.config['airsim']['vehicle_name']
self.logger.info(f"Connected to AirSim at {self.config['airsim']['host']}:{self.config['airsim']['port']}")
except Exception as e:
self.logger.error(f"Failed to connect to AirSim: {e}")
raise
def setup_ros2_publishers(self):
"""初始化ROS2发布器"""
# 配置QoS
qos_profile = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)
self.publishers = {}
# RGB相机发布器
if self.config['sensors']['rgb_camera']['enabled']:
self.publishers['rgb_camera'] = self.create_publisher(
Image,
self.config['sensors']['rgb_camera']['topic'],
qos_profile
)
self.logger.info(f"RGB camera publisher created on topic: {self.config['sensors']['rgb_camera']['topic']}")
# 深度相机发布器
if self.config['sensors']['depth_camera']['enabled']:
self.publishers['depth_camera'] = self.create_publisher(
Image,
self.config['sensors']['depth_camera']['topic'],
qos_profile
)
self.logger.info(f"Depth camera publisher created on topic: {self.config['sensors']['depth_camera']['topic']}")
# 激光雷达发布器
if self.config['sensors']['lidar']['enabled']:
self.publishers['lidar'] = self.create_publisher(
PointCloud2,
self.config['sensors']['lidar']['topic'],
qos_profile
)
self.logger.info(f"LiDAR publisher created on topic: {self.config['sensors']['lidar']['topic']}")
def get_rgb_image(self) -> Optional[np.ndarray]:
"""获取RGB相机图像"""
try:
camera_name = self.config['sensors']['rgb_camera']['camera_name']
response = self.airsim_client.simGetImage(
camera_name,
airsim.ImageType.Scene,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_rgb = cv2.imdecode(img1d, cv2.IMREAD_COLOR)
return cv2.cvtColor(img_rgb, cv2.COLOR_BGR2RGB)
except Exception as e:
self.logger.error(f"Error getting RGB image: {e}")
return None
def get_depth_image(self) -> Optional[np.ndarray]:
"""获取深度相机图像"""
try:
camera_name = self.config['sensors']['depth_camera']['camera_name']
response = self.airsim_client.simGetImage(
camera_name,
airsim.ImageType.DepthPerspective,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_depth = cv2.imdecode(img1d, cv2.IMREAD_GRAYSCALE)
return img_depth
except Exception as e:
self.logger.error(f"Error getting depth image: {e}")
return None
def get_lidar_data(self) -> Optional[np.ndarray]:
"""获取激光雷达数据"""
try:
lidar_name = self.config['sensors']['lidar']['lidar_name']
lidar_data = self.airsim_client.getLidarData(
lidar_name,
vehicle_name=self.vehicle_name
)
if lidar_data.point_cloud_np.size > 0:
return lidar_data.point_cloud_np.reshape(-1, 3)
except Exception as e:
self.logger.error(f"Error getting LiDAR data: {e}")
return None
def publish_rgb_camera(self):
"""发布RGB相机数据"""
rate = self.config['sensors']['rgb_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
rgb_image = self.get_rgb_image()
if rgb_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['rgb_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(rgb_image, "rgb8")
ros_image.header = header
# 发布消息
self.publishers['rgb_camera'].publish(ros_image)
self.logger.debug("Published RGB camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in RGB camera publishing thread: {e}")
time.sleep(1.0)
def publish_depth_camera(self):
"""发布深度相机数据"""
rate = self.config['sensors']['depth_camera']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
depth_image = self.get_depth_image()
if depth_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['depth_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(depth_image, "mono8")
ros_image.header = header
# 发布消息
self.publishers['depth_camera'].publish(ros_image)
self.logger.debug("Published depth camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in depth camera publishing thread: {e}")
time.sleep(1.0)
def publish_lidar(self):
"""发布激光雷达数据"""
rate = self.config['sensors']['lidar']['publish_rate']
sleep_time = 1.0 / rate
while self.running:
try:
lidar_points = self.get_lidar_data()
if lidar_points is not None and lidar_points.size > 0:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['lidar']['frame_id']
# 创建点云消息
point_cloud = pc2.create_cloud_xyz32(header, lidar_points)
# 发布消息
self.publishers['lidar'].publish(point_cloud)
self.logger.debug(f"Published LiDAR point cloud with {len(lidar_points)} points")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in LiDAR publishing thread: {e}")
time.sleep(1.0)
def start_sensor_threads(self):
"""启动传感器数据发布线程"""
if self.config['sensors']['rgb_camera']['enabled']:
rgb_thread = threading.Thread(target=self.publish_rgb_camera, daemon=True)
rgb_thread.start()
self.publish_threads.append(rgb_thread)
self.logger.info("RGB camera publishing thread started")
if self.config['sensors']['depth_camera']['enabled']:
depth_thread = threading.Thread(target=self.publish_depth_camera, daemon=True)
depth_thread.start()
self.publish_threads.append(depth_thread)
self.logger.info("Depth camera publishing thread started")
if self.config['sensors']['lidar']['enabled']:
lidar_thread = threading.Thread(target=self.publish_lidar, daemon=True)
lidar_thread.start()
self.publish_threads.append(lidar_thread)
self.logger.info("LiDAR publishing thread started")
def shutdown(self):
"""关闭发布器"""
self.logger.info("Shutting down AirSim ROS2 Publisher...")
self.running = False
# 等待所有线程结束
for thread in self.publish_threads:
thread.join(timeout=2.0)
# 关闭AirSim连接
try:
self.airsim_client.enableApiControl(False)
except:
pass
self.logger.info("Shutdown complete")
def create_default_config():
"""创建默认配置文件"""
config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
with open('airsim_config.yaml', 'w') as f:
yaml.dump(config, f, default_flow_style=False)
print("Default configuration file created: airsim_config.yaml")
def main():
"""主函数"""
parser = argparse.ArgumentParser(description='AirSim ROS2 Publisher')
parser.add_argument('--config', type=str, help='Configuration file path')
parser.add_argument('--create-config', action='store_true', help='Create default configuration file')
args = parser.parse_args()
if args.create_config:
create_default_config()
return
# 初始化ROS2
rclpy.init()
try:
# 创建发布器节点
publisher = AirSimROS2Publisher(args.config)
# 运行节点
rclpy.spin(publisher)
except KeyboardInterrupt:
print("\nShutdown requested by user")
except Exception as e:
print(f"Error: {e}")
finally:
# 清理
if 'publisher' in locals():
publisher.shutdown()
rclpy.shutdown()
if __name__ == '__main__':
main()
配置
AirSim ROS2 Publisher 使用说明
功能特点
- 多传感器支持: RGB相机、深度相机、激光雷达
- 配置灵活: 通过YAML文件配置所有参数
- 多线程处理: 每个传感器独立线程，互不干扰
- 完整日志: 文件和控制台双重日志记录
- 错误处理: 全面的异常处理和自动重连机制
- ROS2兼容: 标准ROS2消息格式和QoS配置
安装依赖
# 安装Python依赖
pip install airsim rclpy opencv-python numpy pyyaml
# 安装ROS2依赖包
sudo apt install ros-humble-sensor-msgs-py ros-humble-cv-bridge
配置文件示例 (airsim_config.yaml)
airsim:
host: '192.168.1.100' # AirSim服务器IP地址
port: 41451 # AirSim端口
vehicle_name: 'Drone1' # 车辆名称
sensors:
rgb_camera:
enabled: true
camera_name: 'front_center' # AirSim中的相机名称
topic: '/airsim/rgb_camera/image_raw' # ROS话题名称
frame_id: 'rgb_camera_frame' # 坐标系ID
publish_rate: 30.0 # 发布频率(Hz)
depth_camera:
enabled: true
camera_name: 'front_center'
topic: '/airsim/depth_camera/image_raw'
frame_id: 'depth_camera_frame'
publish_rate: 30.0
lidar:
enabled: true
lidar_name: 'Lidar1' # AirSim中的激光雷达名称
topic: '/airsim/lidar/points' # ROS话题名称
frame_id: 'lidar_frame' # 坐标系ID
publish_rate: 10.0 # 发布频率(Hz)
使用方法
1. 创建默认配置文件
python3 airsim_ros2_publisher.py --create-config
2. 编辑配置文件
根据您的AirSim设置修改 airsim_config.yaml 文件
3. 运行发布器
# 使用默认配置
python3 airsim_ros2_publisher.py
# 使用自定义配置文件
python3 airsim_ros2_publisher.py --config /path/to/your/config.yaml
ROS2话题说明
默认话题列表
/airsim/rgb_camera/image_raw- RGB相机图像 (sensor_msgs/Image)/airsim/depth_camera/image_raw- 深度相机图像 (sensor_msgs/Image)/airsim/lidar/points- 激光雷达点云 (sensor_msgs/PointCloud2)
查看话题
# 查看所有话题
ros2 topic list
# 查看话题信息
ros2 topic info /airsim/rgb_camera/image_raw
# 查看话题数据
ros2 topic echo /airsim/rgb_camera/image_raw
在RViz中可视化
# 启动RViz
rviz2
# 添加显示项:
# - Image: 显示相机图像
# - PointCloud2: 显示激光雷达点云
日志文件
程序会生成 airsim_ros2_publisher.log 日志文件，包含：
- 连接状态信息
- 传感器数据发布状态
- 错误信息和警告
- 性能统计信息
常见问题
1. 连接失败
- 检查AirSim是否正在运行
- 确认IP地址和端口配置正确
- 检查网络连接
2. 没有数据发布
- 确认传感器在AirSim中已启用
- 检查传感器名称是否正确
- 查看日志文件了解详细错误信息
3. 性能问题
- 降低发布频率 (publish_rate)
- 禁用不需要的传感器
- 检查网络带宽
扩展功能
添加新传感器类型
- 在配置文件中添加新传感器配置
- 在代码中添加对应的获取和发布函数
- 创建新的发布线程
自定义消息格式
可以根据需要修改消息类型和格式，例如：
- 添加时间戳同步
- 包含位置和姿态信息
- 自定义点云字段
系统要求
- Ubuntu 20.04+
- ROS2 Humble/Iron
- Python 3.8+
- AirSim 1.8.0+
许可证
本代码仅供学习和研究使用。
点击查看代码
需要安装的额外包
由于您已经有了ROS2，还需要安装以下包：
1. Python依赖包
bash# AirSim Python客户端
pip3 install airsim
# 图像处理
pip3 install opencv-python
# 数据处理
pip3 install numpy
# 配置文件支持
pip3 install pyyaml
2. ROS2依赖包
bash# 传感器消息支持
sudo apt install ros-humble-sensor-msgs-py
# 图像消息转换桥接
sudo apt install ros-humble-cv-bridge
# 点云处理工具(可选，用于调试)
sudo apt install ros-humble-pcl-ros
# RViz可视化工具(可选)
sudo apt install ros-humble-rviz2
3. 验证安装
运行程序后，可以用以下命令验证数据发布：
bash# 查看所有话题
ros2 topic list
# 查看特定话题信息
ros2 topic info /airsim/rgb_camera/image_raw
# 检查话题发布频率
ros2 topic hz /airsim/rgb_camera/image_raw
# 查看话题数据(少量)
ros2 topic echo /airsim/rgb_camera/image_raw --once
4. 在RViz中可视化
bash# 启动RViz
ros2 run rviz2 rviz2
# 然后在RViz中添加：
# - Image显示器：订阅 /airsim/rgb_camera/image_raw
# - PointCloud2显示器：订阅 /airsim/lidar/points
这样您就可以在Ubuntu系统上通过ROS2话题接收并使用AirSim的传感器数据了。
点击查看代码
#!/usr/bin/env python3
"""
AirSim传感器数据ROS2发布器
功能：从AirSim获取RGB相机、深度相机和激光雷达数据，并发布到ROS2话题
作者：AI Assistant
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import airsim
import numpy as np
import cv2
from sensor_msgs.msg import Image, PointCloud2, LaserScan
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
import sensor_msgs_py.point_cloud2 as pc2
from cv_bridge import CvBridge
import threading
import time
import logging
from typing import Optional, Dict, Any
import argparse
import yaml
import os
class AirSimROS2Publisher(Node):
"""AirSim传感器数据ROS2发布器类"""
def __init__(self, config_file: str = None):
super().__init__('airsim_ros2_publisher')
# 配置日志
self.setup_logging()
# 加载配置
self.load_config(config_file)
# 初始化AirSim客户端
self.setup_airsim_client()
# 初始化ROS2发布器
self.setup_ros2_publishers()
# 初始化CV Bridge
self.bridge = CvBridge()
# 控制变量
self.running = True
self.publish_threads = []
# 启动传感器数据发布线程
self.start_sensor_threads()
self.get_logger().info("AirSim ROS2 Publisher initialized successfully")
def setup_logging(self):
"""设置日志系统"""
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
handlers=[
logging.FileHandler('airsim_ros2_publisher.log'),
logging.StreamHandler()
]
)
self.logger = logging.getLogger(__name__)
def load_config(self, config_file: str):
"""加载配置文件"""
default_config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
if config_file and os.path.exists(config_file):
try:
with open(config_file, 'r') as f:
user_config = yaml.safe_load(f)
# 合并配置
self.config = self.merge_configs(default_config, user_config)
self.logger.info(f"Loaded configuration from {config_file}")
except Exception as e:
self.logger.error(f"Error loading config file: {e}")
self.config = default_config
else:
self.config = default_config
self.logger.info("Using default configuration")
def merge_configs(self, default: Dict, user: Dict) -> Dict:
"""合并配置字典"""
result = default.copy()
for key, value in user.items():
if key in result and isinstance(result[key], dict) and isinstance(value, dict):
result[key] = self.merge_configs(result[key], value)
else:
result[key] = value
return result
def setup_airsim_client(self):
"""初始化AirSim客户端"""
try:
# 主客户端用于初始化连接验证
self.airsim_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.airsim_client.confirmConnection()
self.airsim_client.enableApiControl(True)
self.vehicle_name = self.config['airsim']['vehicle_name']
# 为每个传感器创建独立的客户端连接
self.rgb_client = None
self.depth_client = None
self.lidar_client = None
# 根据启用的传感器创建客户端
if self.config['sensors']['rgb_camera']['enabled']:
self.rgb_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.rgb_client.confirmConnection()
if self.config['sensors']['depth_camera']['enabled']:
self.depth_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.depth_client.confirmConnection()
if self.config['sensors']['lidar']['enabled']:
self.lidar_client = airsim.MultirotorClient(
ip=self.config['airsim']['host'],
port=self.config['airsim']['port']
)
self.lidar_client.confirmConnection()
self.logger.info(f"Connected to AirSim at {self.config['airsim']['host']}:{self.config['airsim']['port']}")
except Exception as e:
self.logger.error(f"Failed to connect to AirSim: {e}")
raise
def setup_ros2_publishers(self):
"""初始化ROS2发布器"""
# 配置QoS
qos_profile = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=1
)
# 避免与Node基类的publishers属性冲突，使用不同的名称
self.sensor_publishers = {}
# RGB相机发布器
if self.config['sensors']['rgb_camera']['enabled']:
self.sensor_publishers['rgb_camera'] = self.create_publisher(
Image,
self.config['sensors']['rgb_camera']['topic'],
qos_profile
)
self.logger.info(f"RGB camera publisher created on topic: {self.config['sensors']['rgb_camera']['topic']}")
# 深度相机发布器
if self.config['sensors']['depth_camera']['enabled']:
self.sensor_publishers['depth_camera'] = self.create_publisher(
Image,
self.config['sensors']['depth_camera']['topic'],
qos_profile
)
self.logger.info(f"Depth camera publisher created on topic: {self.config['sensors']['depth_camera']['topic']}")
# 激光雷达发布器
if self.config['sensors']['lidar']['enabled']:
self.sensor_publishers['lidar'] = self.create_publisher(
PointCloud2,
self.config['sensors']['lidar']['topic'],
qos_profile
)
self.logger.info(f"LiDAR publisher created on topic: {self.config['sensors']['lidar']['topic']}")
def get_rgb_image(self) -> Optional[np.ndarray]:
"""获取RGB相机图像"""
try:
if not self.rgb_client:
return None
camera_name = self.config['sensors']['rgb_camera']['camera_name']
response = self.rgb_client.simGetImage(
camera_name,
airsim.ImageType.Scene,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_rgb = cv2.imdecode(img1d, cv2.IMREAD_COLOR)
return cv2.cvtColor(img_rgb, cv2.COLOR_BGR2RGB)
except Exception as e:
self.logger.error(f"Error getting RGB image: {e}")
return None
def get_depth_image(self) -> Optional[np.ndarray]:
"""获取深度相机图像"""
try:
if not self.depth_client:
return None
camera_name = self.config['sensors']['depth_camera']['camera_name']
response = self.depth_client.simGetImage(
camera_name,
airsim.ImageType.DepthPerspective,
vehicle_name=self.vehicle_name
)
if response:
# 转换为numpy数组
img1d = np.frombuffer(response, dtype=np.uint8)
img_depth = cv2.imdecode(img1d, cv2.IMREAD_GRAYSCALE)
return img_depth
except Exception as e:
self.logger.error(f"Error getting depth image: {e}")
return None
def get_lidar_data(self) -> Optional[np.ndarray]:
"""获取激光雷达数据"""
try:
if not self.lidar_client:
return None
lidar_name = self.config['sensors']['lidar']['lidar_name']
lidar_data = self.lidar_client.getLidarData(
lidar_name,
vehicle_name=self.vehicle_name
)
if lidar_data.point_cloud_np.size > 0:
return lidar_data.point_cloud_np.reshape(-1, 3)
except Exception as e:
self.logger.error(f"Error getting LiDAR data: {e}")
return None
def publish_rgb_camera(self):
"""发布RGB相机数据"""
rate = float(self.config['sensors']['rgb_camera']['publish_rate'])
sleep_time = 1.0 / rate
while self.running:
try:
rgb_image = self.get_rgb_image()
if rgb_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['rgb_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(rgb_image, "rgb8")
ros_image.header = header
# 发布消息
self.sensor_publishers['rgb_camera'].publish(ros_image)
self.logger.debug("Published RGB camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in RGB camera publishing thread: {e}")
time.sleep(1.0)
def publish_depth_camera(self):
"""发布深度相机数据"""
rate = float(self.config['sensors']['depth_camera']['publish_rate'])
sleep_time = 1.0 / rate
while self.running:
try:
depth_image = self.get_depth_image()
if depth_image is not None:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['depth_camera']['frame_id']
ros_image = self.bridge.cv2_to_imgmsg(depth_image, "mono8")
ros_image.header = header
# 发布消息
self.sensor_publishers['depth_camera'].publish(ros_image)
self.logger.debug("Published depth camera image")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in depth camera publishing thread: {e}")
time.sleep(1.0)
def publish_lidar(self):
"""发布激光雷达数据"""
rate = float(self.config['sensors']['lidar']['publish_rate'])
sleep_time = 1.0 / rate
while self.running:
try:
lidar_points = self.get_lidar_data()
if lidar_points is not None and lidar_points.size > 0:
# 创建ROS消息
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = self.config['sensors']['lidar']['frame_id']
# 创建点云消息
point_cloud = pc2.create_cloud_xyz32(header, lidar_points)
# 发布消息
self.sensor_publishers['lidar'].publish(point_cloud)
self.logger.debug(f"Published LiDAR point cloud with {len(lidar_points)} points")
time.sleep(sleep_time)
except Exception as e:
self.logger.error(f"Error in LiDAR publishing thread: {e}")
time.sleep(1.0)
def start_sensor_threads(self):
"""启动传感器数据发布线程"""
if self.config['sensors']['rgb_camera']['enabled']:
rgb_thread = threading.Thread(target=self.publish_rgb_camera, daemon=True)
rgb_thread.start()
self.publish_threads.append(rgb_thread)
self.logger.info("RGB camera publishing thread started")
if self.config['sensors']['depth_camera']['enabled']:
depth_thread = threading.Thread(target=self.publish_depth_camera, daemon=True)
depth_thread.start()
self.publish_threads.append(depth_thread)
self.logger.info("Depth camera publishing thread started")
if self.config['sensors']['lidar']['enabled']:
lidar_thread = threading.Thread(target=self.publish_lidar, daemon=True)
lidar_thread.start()
self.publish_threads.append(lidar_thread)
self.logger.info("LiDAR publishing thread started")
def shutdown(self):
"""关闭发布器"""
self.logger.info("Shutting down AirSim ROS2 Publisher...")
self.running = False
# 等待所有线程结束
for thread in self.publish_threads:
thread.join(timeout=2.0)
# 关闭所有AirSim连接
try:
if self.airsim_client:
self.airsim_client.enableApiControl(False)
if self.rgb_client:
del self.rgb_client
if self.depth_client:
del self.depth_client
if self.lidar_client:
del self.lidar_client
except:
pass
self.logger.info("Shutdown complete")
def create_default_config():
"""创建默认配置文件"""
config = {
'airsim': {
'host': '127.0.0.1',
'port': 41451,
'vehicle_name': 'Drone1'
},
'sensors': {
'rgb_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/rgb_camera/image_raw',
'frame_id': 'rgb_camera_frame',
'publish_rate': 10.0
},
'depth_camera': {
'enabled': True,
'camera_name': 'front_center',
'topic': '/airsim/depth_camera/image_raw',
'frame_id': 'depth_camera_frame',
'publish_rate': 10.0
},
'lidar': {
'enabled': True,
'lidar_name': 'Lidar1',
'topic': '/airsim/lidar/points',
'frame_id': 'lidar_frame',
'publish_rate': 10.0
}
}
}
with open('airsim_config.yaml', 'w') as f:
yaml.dump(config, f, default_flow_style=False)
print("Default configuration file created: airsim_config.yaml")
def main():
"""主函数"""
parser = argparse.ArgumentParser(description='AirSim ROS2 Publisher')
parser.add_argument('--config', type=str, help='Configuration file path')
parser.add_argument('--create-config', action='store_true', help='Create default configuration file')
args = parser.parse_args()
if args.create_config:
create_default_config()
return
# 初始化ROS2
rclpy.init()
try:
# 创建发布器节点
publisher = AirSimROS2Publisher(args.config)
# 运行节点
rclpy.spin(publisher)
except KeyboardInterrupt:
print("\nShutdown requested by user")
except Exception as e:
print(f"Error: {e}")
finally:
# 清理
if 'publisher' in locals():
publisher.shutdown()
rclpy.shutdown()
if __name__ == '__main__':
main()
浙公网安备 33010602011771号