From 02b37ee4bd81512adaf892496d9b627a68426969 Mon Sep 17 00:00:00 2001
From: dream1112221 <2199305792@qq.com>
Date: Thu, 18 Dec 2025 21:22:48 +0800
Subject: [PATCH 1/4] =?UTF-8?q?=E8=AE=BE=E7=BD=AE=E4=B8=89=E7=A7=8D?=
=?UTF-8?q?=E5=B7=A5=E4=BD=9C=E6=A8=A1=E5=BC=8F=E4=B8=8B=E7=9A=84=E5=AF=BC?=
=?UTF-8?q?=E8=88=AA=E7=B3=BB=E7=BB=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../carla_navigation.launch | 231 ++++++++++++++++++
.../carla_slam_navigation.launch | 185 ++++++++++++++
2 files changed, 416 insertions(+)
create mode 100644 src/carla_slam_gmapping/carla_navigation.launch
create mode 100644 src/carla_slam_gmapping/carla_slam_navigation.launch
diff --git a/src/carla_slam_gmapping/carla_navigation.launch b/src/carla_slam_gmapping/carla_navigation.launch
new file mode 100644
index 0000000000..50e1b10ea2
--- /dev/null
+++ b/src/carla_slam_gmapping/carla_navigation.launch
@@ -0,0 +1,231 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/carla_slam_gmapping/carla_slam_navigation.launch b/src/carla_slam_gmapping/carla_slam_navigation.launch
new file mode 100644
index 0000000000..158c263aeb
--- /dev/null
+++ b/src/carla_slam_gmapping/carla_slam_navigation.launch
@@ -0,0 +1,185 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
From 99a7e96be841ca78c4424d75f1945348c89df1db Mon Sep 17 00:00:00 2001
From: dream1112221 <2199305792@qq.com>
Date: Fri, 19 Dec 2025 10:30:59 +0800
Subject: [PATCH 2/4] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=85=8D=E7=BD=AE?=
=?UTF-8?q?=E6=96=87=E4=BB=B6=EF=BC=8C=E5=AE=8C=E5=96=84=E5=AF=BC=E8=88=AA?=
=?UTF-8?q?=E7=B3=BB=E7=BB=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../costmap_common_params.yaml | 24 +++++++++
.../dwa_local_planner_params.yaml | 52 +++++++++++++++++++
.../global_costmap_params.yaml | 19 +++++++
.../local_costmap_params.yaml | 17 ++++++
src/carla_slam_gmapping/move_base_params.yaml | 29 +++++++++++
5 files changed, 141 insertions(+)
create mode 100644 src/carla_slam_gmapping/costmap_common_params.yaml
create mode 100644 src/carla_slam_gmapping/dwa_local_planner_params.yaml
create mode 100644 src/carla_slam_gmapping/global_costmap_params.yaml
create mode 100644 src/carla_slam_gmapping/local_costmap_params.yaml
create mode 100644 src/carla_slam_gmapping/move_base_params.yaml
diff --git a/src/carla_slam_gmapping/costmap_common_params.yaml b/src/carla_slam_gmapping/costmap_common_params.yaml
new file mode 100644
index 0000000000..d339aff288
--- /dev/null
+++ b/src/carla_slam_gmapping/costmap_common_params.yaml
@@ -0,0 +1,24 @@
+# Common costmap parameters for both global and local costmaps
+
+# Obstacle marking parameters
+obstacle_range: 50.0
+raytrace_range: 100.0
+
+# Robot footprint (Tesla Model3 approximate dimensions)
+footprint: [[-2.0, -1.0], [-2.0, 1.0], [2.5, 1.0], [2.5, -1.0]]
+inflation_radius: 1.5
+cost_scaling_factor: 10.0
+
+# Observation sources
+observation_sources: laser_scan_sensor
+
+laser_scan_sensor:
+ sensor_frame: ego_vehicle/lidar
+ data_type: LaserScan
+ topic: /carla/ego_vehicle/scan
+ marking: true
+ clearing: true
+ min_obstacle_height: -0.5
+ max_obstacle_height: 2.0
+ obstacle_range: 50.0
+ raytrace_range: 100.0
diff --git a/src/carla_slam_gmapping/dwa_local_planner_params.yaml b/src/carla_slam_gmapping/dwa_local_planner_params.yaml
new file mode 100644
index 0000000000..6b7407b833
--- /dev/null
+++ b/src/carla_slam_gmapping/dwa_local_planner_params.yaml
@@ -0,0 +1,52 @@
+# DWA Local Planner parameters
+
+DWAPlannerROS:
+ # Robot configuration (Tesla Model3 specs)
+ max_vel_x: 10.0
+ min_vel_x: -2.0
+ max_vel_y: 0.0
+ min_vel_y: 0.0
+
+ max_trans_vel: 10.0
+ min_trans_vel: 0.5
+ trans_stopped_vel: 0.1
+
+ max_rot_vel: 1.0
+ min_rot_vel: 0.2
+ rot_stopped_vel: 0.1
+
+ # Acceleration limits
+ acc_lim_x: 2.5
+ acc_lim_y: 0.0
+ acc_lim_theta: 3.0
+ decel_lim_x: -2.5
+ decel_lim_theta: -3.0
+
+ # Goal tolerance
+ xy_goal_tolerance: 0.5
+ yaw_goal_tolerance: 0.1
+ latch_xy_goal_tolerance: false
+
+ # Forward simulation
+ sim_time: 2.0
+ sim_granularity: 0.025
+ vx_samples: 20
+ vy_samples: 0
+ vtheta_samples: 40
+
+ # Trajectory scoring
+ path_distance_bias: 32.0
+ goal_distance_bias: 20.0
+ occdist_scale: 0.02
+ forward_point_distance: 0.325
+ stop_time_buffer: 0.2
+ scaling_speed: 0.25
+ max_scaling_factor: 0.2
+
+ # Oscillation prevention
+ oscillation_reset_dist: 0.05
+ oscillation_reset_angle: 0.2
+
+ # Debugging
+ publish_traj_pc: true
+ publish_cost_grid_pc: true
diff --git a/src/carla_slam_gmapping/global_costmap_params.yaml b/src/carla_slam_gmapping/global_costmap_params.yaml
new file mode 100644
index 0000000000..1a4fb93ece
--- /dev/null
+++ b/src/carla_slam_gmapping/global_costmap_params.yaml
@@ -0,0 +1,19 @@
+# Global costmap parameters
+
+global_costmap:
+ global_frame: map
+ robot_base_frame: ego_vehicle
+ update_frequency: 2.0
+ publish_frequency: 1.0
+ static_map: true
+
+ # Use rolling window for exploration mode
+ rolling_window: false
+ width: 200.0
+ height: 200.0
+ resolution: 0.05
+
+ plugins:
+ - {name: static_layer, type: "costmap_2d::StaticLayer"}
+ - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
+ - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
diff --git a/src/carla_slam_gmapping/local_costmap_params.yaml b/src/carla_slam_gmapping/local_costmap_params.yaml
new file mode 100644
index 0000000000..9226cd29ba
--- /dev/null
+++ b/src/carla_slam_gmapping/local_costmap_params.yaml
@@ -0,0 +1,17 @@
+# Local costmap parameters
+
+local_costmap:
+ global_frame: odom
+ robot_base_frame: ego_vehicle
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ static_map: false
+ rolling_window: true
+
+ width: 20.0
+ height: 20.0
+ resolution: 0.05
+
+ plugins:
+ - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
+ - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
diff --git a/src/carla_slam_gmapping/move_base_params.yaml b/src/carla_slam_gmapping/move_base_params.yaml
new file mode 100644
index 0000000000..109571fa59
--- /dev/null
+++ b/src/carla_slam_gmapping/move_base_params.yaml
@@ -0,0 +1,29 @@
+# Move base parameters
+
+# Controller frequency
+controller_frequency: 10.0
+controller_patience: 15.0
+
+# Planner frequency
+planner_frequency: 5.0
+planner_patience: 5.0
+
+# Oscillation parameters
+oscillation_timeout: 10.0
+oscillation_distance: 0.2
+
+# Recovery behaviors
+recovery_behavior_enabled: true
+clearing_rotation_allowed: true
+
+recovery_behaviors:
+ - name: 'conservative_reset'
+ type: 'clear_costmap_recovery/ClearCostmapRecovery'
+ - name: 'aggressive_reset'
+ type: 'clear_costmap_recovery/ClearCostmapRecovery'
+
+conservative_reset:
+ reset_distance: 3.0
+
+aggressive_reset:
+ reset_distance: 5.0
From d99b1c3fb4f0b7bcc84001623e0a4d947a772b01 Mon Sep 17 00:00:00 2001
From: dream1112221 <2199305792@qq.com>
Date: Fri, 19 Dec 2025 15:29:08 +0800
Subject: [PATCH 3/4] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E7=9B=AE=E6=A0=87?=
=?UTF-8?q?=E7=82=B9=E5=8F=91=E5=B8=83=E8=8A=82=E7=82=B9=E4=BB=A3=E7=A0=81?=
=?UTF-8?q?=EF=BC=8C=E5=AE=8C=E5=96=84=E5=AF=BC=E8=88=AA=E7=B3=BB=E7=BB=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/carla_slam_gmapping/goal_publisher.py | 157 ++++++++++++++++++++++
1 file changed, 157 insertions(+)
create mode 100755 src/carla_slam_gmapping/goal_publisher.py
diff --git a/src/carla_slam_gmapping/goal_publisher.py b/src/carla_slam_gmapping/goal_publisher.py
new file mode 100755
index 0000000000..ae779e7543
--- /dev/null
+++ b/src/carla_slam_gmapping/goal_publisher.py
@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+"""
+导航目标点发布器
+功能:向move_base发布导航目标点
+使用:python3 goal_publisher.py [x] [y] [yaw]
+"""
+
+import rospy
+import sys
+from geometry_msgs.msg import PoseStamped
+from move_base_msgs.msg import MoveBaseActionGoal
+import math
+
+
+class GoalPublisher:
+ def __init__(self):
+ rospy.init_node('goal_publisher', anonymous=True)
+
+ # 发布器
+ self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
+
+ rospy.loginfo("Goal Publisher initialized")
+ rospy.sleep(1.0) # 等待发布器准备好
+
+ def publish_goal(self, x, y, yaw=0.0):
+ """
+ 发布导航目标点
+
+ Args:
+ x: 目标点X坐标(米)
+ y: 目标点Y坐标(米)
+ yaw: 目标朝向(弧度)
+ """
+ goal = PoseStamped()
+ goal.header.frame_id = "map"
+ goal.header.stamp = rospy.Time.now()
+
+ # 位置
+ goal.pose.position.x = x
+ goal.pose.position.y = y
+ goal.pose.position.z = 0.0
+
+ # 朝向(欧拉角转四元数)
+ goal.pose.orientation.x = 0.0
+ goal.pose.orientation.y = 0.0
+ goal.pose.orientation.z = math.sin(yaw / 2.0)
+ goal.pose.orientation.w = math.cos(yaw / 2.0)
+
+ rospy.loginfo(f"Publishing goal: x={x}, y={y}, yaw={yaw:.2f} rad")
+ self.goal_pub.publish(goal)
+ rospy.loginfo("Goal published successfully!")
+
+ def publish_goal_sequence(self, goals):
+ """
+ 发布一系列导航目标点(按顺序)
+
+ Args:
+ goals: 目标点列表,每个元素为 (x, y, yaw)
+ """
+ rospy.loginfo(f"Publishing {len(goals)} goals in sequence...")
+
+ for i, (x, y, yaw) in enumerate(goals):
+ rospy.loginfo(f"Goal {i+1}/{len(goals)}")
+ self.publish_goal(x, y, yaw)
+
+ # 等待用户确认或超时
+ input(f"Press Enter to send next goal (or Ctrl+C to stop)...")
+
+
+def predefined_goals():
+ """
+ 预定义的测试目标点集合(Town01场景)
+ """
+ return {
+ "straight": [
+ (10.0, 0.0, 0.0),
+ (20.0, 0.0, 0.0),
+ (30.0, 0.0, 0.0),
+ ],
+ "square": [
+ (20.0, 0.0, 0.0),
+ (20.0, 20.0, 1.57),
+ (0.0, 20.0, 3.14),
+ (0.0, 0.0, -1.57),
+ ],
+ "explore": [
+ (50.0, 0.0, 0.0),
+ (50.0, 50.0, 1.57),
+ (-50.0, 50.0, 3.14),
+ (-50.0, -50.0, -1.57),
+ (0.0, 0.0, 0.0),
+ ],
+ }
+
+
+def main():
+ if len(sys.argv) < 2:
+ print("\n=== Goal Publisher Usage ===")
+ print("1. Single goal:")
+ print(" python3 goal_publisher.py [yaw]")
+ print(" Example: python3 goal_publisher.py 10.0 5.0 1.57")
+ print("\n2. Predefined goal sequences:")
+ print(" python3 goal_publisher.py straight # 直线前进")
+ print(" python3 goal_publisher.py square # 方形路径")
+ print(" python3 goal_publisher.py explore # 探索路径")
+ print("\n3. Interactive mode:")
+ print(" python3 goal_publisher.py interactive")
+ return
+
+ try:
+ publisher = GoalPublisher()
+
+ # 预定义路径
+ if sys.argv[1] in predefined_goals():
+ goals = predefined_goals()[sys.argv[1]]
+ publisher.publish_goal_sequence(goals)
+
+ # 交互模式
+ elif sys.argv[1] == "interactive":
+ print("\n=== Interactive Goal Publisher ===")
+ print("Enter goal coordinates (or 'q' to quit)")
+ while not rospy.is_shutdown():
+ try:
+ x = input("X coordinate (m): ")
+ if x.lower() == 'q':
+ break
+ y = input("Y coordinate (m): ")
+ yaw = input("Yaw angle (rad, default=0.0): ")
+
+ x = float(x)
+ y = float(y)
+ yaw = float(yaw) if yaw else 0.0
+
+ publisher.publish_goal(x, y, yaw)
+ except ValueError:
+ print("Invalid input! Please enter numbers.")
+ except KeyboardInterrupt:
+ break
+
+ # 单个目标点
+ else:
+ x = float(sys.argv[1])
+ y = float(sys.argv[2])
+ yaw = float(sys.argv[3]) if len(sys.argv) > 3 else 0.0
+
+ publisher.publish_goal(x, y, yaw)
+
+ except rospy.ROSInterruptException:
+ rospy.loginfo("Goal Publisher shutdown")
+ except Exception as e:
+ rospy.logerr(f"Error: {e}")
+
+
+if __name__ == '__main__':
+ main()
From a76c8b72caca8fd61ef8c012ff464d9eba45caef Mon Sep 17 00:00:00 2001
From: dream1112221 <2199305792@qq.com>
Date: Fri, 19 Dec 2025 17:44:02 +0800
Subject: [PATCH 4/4] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E7=AE=A1=E7=90=86CARLA?=
=?UTF-8?q?=E4=BB=BF=E7=9C=9F=E5=9C=BA=E6=99=AF=E4=BB=A3=E7=A0=81=EF=BC=8C?=
=?UTF-8?q?=E5=AE=8C=E5=96=84=E5=AF=BC=E8=88=AA=E2=9E=95=E5=BB=BA=E5=9B=BE?=
=?UTF-8?q?=E7=B3=BB=E7=BB=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/carla_slam_gmapping/scenario_manager.py | 185 ++++++++++++++++++++
1 file changed, 185 insertions(+)
create mode 100755 src/carla_slam_gmapping/scenario_manager.py
diff --git a/src/carla_slam_gmapping/scenario_manager.py b/src/carla_slam_gmapping/scenario_manager.py
new file mode 100755
index 0000000000..65ba49c4a5
--- /dev/null
+++ b/src/carla_slam_gmapping/scenario_manager.py
@@ -0,0 +1,185 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+"""
+CARLA场景管理器
+功能:切换CARLA地图,支持不同场景的重复实验
+"""
+
+import rospy
+import carla
+import sys
+import time
+
+
+class ScenarioManager:
+ def __init__(self, host='localhost', port=2000, timeout=10.0):
+ """
+ 初始化场景管理器
+
+ Args:
+ host: CARLA服务器地址
+ port: CARLA服务器端口
+ timeout: 连接超时时间
+ """
+ try:
+ self.client = carla.Client(host, port)
+ self.client.set_timeout(timeout)
+ self.world = self.client.get_world()
+ print(f"Connected to CARLA server at {host}:{port}")
+ print(f"Current map: {self.world.get_map().name}")
+ except Exception as e:
+ print(f"Failed to connect to CARLA: {e}")
+ sys.exit(1)
+
+ def list_available_maps(self):
+ """列出所有可用的地图"""
+ maps = self.client.get_available_maps()
+ print("\n=== Available Maps ===")
+ for i, map_name in enumerate(maps, 1):
+ print(f"{i}. {map_name}")
+ return maps
+
+ def switch_map(self, map_name):
+ """
+ 切换地图
+
+ Args:
+ map_name: 地图名称(如 'Town01', 'Town02' 等)
+ """
+ print(f"\nSwitching to map: {map_name}")
+ try:
+ # 如果不包含完整路径,自动补全
+ if not map_name.startswith('/Game/Carla/Maps'):
+ map_name = f'/Game/Carla/Maps/{map_name}'
+
+ self.world = self.client.load_world(map_name)
+ print(f"Map switched successfully!")
+ print(f"Current map: {self.world.get_map().name}")
+ time.sleep(2) # 等待地图加载完成
+ return True
+ except Exception as e:
+ print(f"Failed to switch map: {e}")
+ return False
+
+ def get_spawn_points(self):
+ """获取当前地图的所有出生点"""
+ spawn_points = self.world.get_map().get_spawn_points()
+ print(f"\n=== Spawn Points ({len(spawn_points)} total) ===")
+ for i, point in enumerate(spawn_points[:10], 1): # 只显示前10个
+ loc = point.location
+ print(f"{i}. x={loc.x:.2f}, y={loc.y:.2f}, z={loc.z:.2f}, yaw={point.rotation.yaw:.2f}")
+ if len(spawn_points) > 10:
+ print(f"... and {len(spawn_points) - 10} more")
+ return spawn_points
+
+ def set_weather(self, weather_preset='ClearNoon'):
+ """
+ 设置天气
+
+ Args:
+ weather_preset: 天气预设名称
+ """
+ weather_presets = {
+ 'ClearNoon': carla.WeatherParameters.ClearNoon,
+ 'CloudyNoon': carla.WeatherParameters.CloudyNoon,
+ 'WetNoon': carla.WeatherParameters.WetNoon,
+ 'WetCloudyNoon': carla.WeatherParameters.WetCloudyNoon,
+ 'SoftRainNoon': carla.WeatherParameters.SoftRainNoon,
+ 'MidRainyNoon': carla.WeatherParameters.MidRainyNoon,
+ 'HardRainNoon': carla.WeatherParameters.HardRainNoon,
+ 'ClearSunset': carla.WeatherParameters.ClearSunset,
+ 'CloudySunset': carla.WeatherParameters.CloudySunset,
+ 'WetSunset': carla.WeatherParameters.WetSunset,
+ }
+
+ if weather_preset in weather_presets:
+ self.world.set_weather(weather_presets[weather_preset])
+ print(f"Weather set to: {weather_preset}")
+ else:
+ print(f"Unknown weather preset: {weather_preset}")
+ print(f"Available presets: {', '.join(weather_presets.keys())}")
+
+
+def main():
+ if len(sys.argv) < 2:
+ print("\n=== Scenario Manager Usage ===")
+ print("1. List available maps:")
+ print(" python3 scenario_manager.py list")
+ print("\n2. Switch to a specific map:")
+ print(" python3 scenario_manager.py switch ")
+ print(" Example: python3 scenario_manager.py switch Town02")
+ print("\n3. Show spawn points:")
+ print(" python3 scenario_manager.py spawns")
+ print("\n4. Set weather:")
+ print(" python3 scenario_manager.py weather ")
+ print(" Example: python3 scenario_manager.py weather CloudyNoon")
+ print("\n5. Interactive mode:")
+ print(" python3 scenario_manager.py interactive")
+ return
+
+ try:
+ manager = ScenarioManager()
+
+ command = sys.argv[1].lower()
+
+ if command == "list":
+ manager.list_available_maps()
+
+ elif command == "switch":
+ if len(sys.argv) < 3:
+ print("Error: Please specify map name")
+ print("Example: python3 scenario_manager.py switch Town02")
+ else:
+ manager.switch_map(sys.argv[2])
+
+ elif command == "spawns":
+ manager.get_spawn_points()
+
+ elif command == "weather":
+ if len(sys.argv) < 3:
+ print("Error: Please specify weather preset")
+ else:
+ manager.set_weather(sys.argv[2])
+
+ elif command == "interactive":
+ print("\n=== Interactive Scenario Manager ===")
+ manager.list_available_maps()
+
+ while True:
+ print("\nCommands:")
+ print(" switch