Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
67 changes: 66 additions & 1 deletion src/carla_slam_gmapping/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,14 +8,17 @@
- 改进的错误处理
- 性能统计
- 配置参数化
- SLAM功能集成
"""

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from nav_msgs.msg import Path, OccupancyGrid
from geometry_msgs.msg import PoseStamped, Point
from visualization_msgs.msg import Marker, MarkerArray
from carla_global_planner.srv import PlanGlobalPath
from sensor_msgs.msg import PointCloud2
from std_msgs.msg import String
from utilities.planner import compute_route_waypoints

import carla
Expand Down Expand Up @@ -44,9 +47,17 @@ def __init__(self):
('path_line_width', 0.6), # 路径可视化线宽
('enable_performance_stats', True), # 是否启用性能统计
('publish_goal_marker', True), # 是否发布目标点标记
('use_slam', False), # 是否启用SLAM功能
('slam_topic', '/slam/map'), # SLAM地图话题
('pointcloud_topic', '/velodyne_points'), # 点云话题
]
)

# 新增:SLAM相关属性
self.use_slam = self.get_parameter('use_slam').value
self.slam_map = None
self.pointcloud_data = None

# 新增:性能统计变量
self.planning_count = 0
self.total_planning_time = 0.0
Expand All @@ -67,6 +78,25 @@ def __init__(self):
self.marker_array_pub = self.create_publisher(
MarkerArray, 'visualization_marker_array', 10)

# 新增:SLAM地图订阅
if self.use_slam:
self.slam_sub = self.create_subscription(
OccupancyGrid,
self.get_parameter('slam_topic').value,
self.slam_callback,
10)

# 新增:点云数据订阅
self.pointcloud_sub = self.create_subscription(
PointCloud2,
self.get_parameter('pointcloud_topic').value,
self.pointcloud_callback,
10)

# 新增:SLAM状态发布
self.slam_status_pub = self.create_publisher(
String, 'slam_status', 10)

self.srv = self.create_service(
PlanGlobalPath, 'plan_to_random_goal', self.plan_path_cb)

Expand All @@ -80,6 +110,25 @@ def __init__(self):
30.0, self._publish_performance_stats)

self.get_logger().info("CARLA全局路径规划服务已启动 [增强版]")
if self.use_slam:
self.get_logger().info("SLAM功能已启用")

def slam_callback(self, msg):
"""SLAM地图回调函数"""
self.slam_map = msg
self.get_logger().debug(f"接收到SLAM地图数据: {msg.info.width}x{msg.info.height}")

def pointcloud_callback(self, msg):
"""点云数据回调函数"""
self.pointcloud_data = msg
self.get_logger().debug(f"接收到点云数据: {msg.height}x{msg.width}")

def publish_slam_status(self, status):
"""发布SLAM状态信息"""
if self.use_slam:
status_msg = String()
status_msg.data = status
self.slam_status_pub.publish(status_msg)

def _initialize_carla_client(self):
"""初始化CARLA客户端连接 - 增强版错误处理"""
Expand Down Expand Up @@ -146,6 +195,15 @@ def plan_path_cb(self, request, response):
response.success = False # 假设服务定义中有success字段
return response

# 新增:检查SLAM状态
if self.use_slam:
if self.slam_map is None:
self.get_logger().warning("SLAM地图尚未准备好")
self.publish_slam_status("waiting_for_map")
else:
self.publish_slam_status("map_ready")
self.get_logger().info("使用SLAM地图进行路径规划")

try:
# 将ROS坐标转换为CARLA坐标
start_location = self._ros_to_carla_location(request.start)
Expand Down Expand Up @@ -185,10 +243,17 @@ def plan_path_cb(self, request, response):
f"总距离: {total_distance:.2f}m, "
f"用时: {planning_time:.3f}s"
)

# 新增:SLAM状态更新
if self.use_slam:
self.publish_slam_status("path_planned")

return response

except Exception as e:
self.get_logger().error(f"路径规划过程中发生错误: {str(e)}")
if self.use_slam:
self.publish_slam_status("planning_error")
return response

def _ros_to_carla_location(self, odom_msg):
Expand Down
Loading