[CARLA] CARLA Simulator Slam

[김위제]

Ubuntu 24.04 Desktop
ros2 jazzy

ros2 jazzy cartographer를 사용하여 맵 만들 것.

$ sudo apt install ros-jazzy-cartographer
$ sudo apt install ros-jazzy-cartographer-ros

https://docs.ros.org/en/jazzy/p/pointcloud_to_laserscan/

pointcloud_to_laserscan — pointcloud_to_laserscan: Jazzy 2.0.2 documentation

laserscan을 사용하기 위해 
3D pointcloud를 2D pointcloud로 바꿔주는 
sudo apt install ros-jazzy-pointcloud-to-laserscan 패키지 install

파라미터로 설정한
min_height와 max_height 사이의 점들만 남기고 나머지는 버립니다.

높이 필터링을 거친 점들은 먼저 z축 값이 제거되어 평면에 투영
그 후, 각 점의 위치를 각도와 거리로 변환  레이저 스캔의 각 채널별로 가장 가까운 점의 거리값을 선택하여 최종적인 2D 스캔 데이터로 만든다

파라미터명

설명

1.

ros2 run pointcloud_to_laserscan pointcloud_to_laserscan_node   --ros-args -p use_sim_time:=True   -p target_frame:=hero   -p min_height:=0.5   -p max_height:=1.5   -p range_min:=1.0   -p range_max:=80.0   -r cloud_in:=/carla/hero/lidar -r scan:=/scan

2.

ros2 run cartographer_ros cartographer_node   -configuration_directory ./   -configuration_basename carla_2D.lua   --ros-args -p use_sim_time:=True   -r imu:=/carla/hero/imu   -r scan:=/scan   -r odom:=/carla/hero/odometry

carla_2D.lua 설정파일

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "hero",
published_frame = "hero",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = true,
use_pose_extrapolator = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

-- 2D Trajectory 설정
TRAJECTORY_BUILDER_2D.min_range = 0.5
TRAJECTORY_BUILDER_2D.max_range = 100.0
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false

-- 스캔 매칭 강화
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)

-- 너무 잦은 업데이트 방지 (0.1도 -> 1.0도)
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.0)

-- 루프 클로저설정
POSE_GRAPH.constraint_builder.min_score = 0.55
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6

return options

파라미터 항목	설정값	상세 설명 및 역할
map_frame	"map"	세계 지도(World)의 절대 기준점 이름
tracking_frame	"hero"	센서(Lidar, IMU) 데이터의 기준이 되는 로봇 중심점
published_frame	"hero"	Cartographer가 최종 위치를 계산해서 발표할 기준 좌표계
odom_frame	"odom"	바퀴 회전 등을 통해 얻은 오도메트리의 기준점
provide_odom_frame	true	핵심: Cartographer가 직접 map → odom 연결(TF)을 생성
use_odometry	true	사용자님이 파이썬 노드로 만든 오도메트리 데이터를 활용
use_imu_data	false	현재 IMU(가속도계)는 쓰지 않고 라이다와 오도메트리만 사용
num_laser_scans	1	한 개의 라이다(LaserScan) 입력을 처리
min_range	0.5	0.5m보다 가까운 장애물은 노이즈로 판단해 무시
max_range	100.0	100m까지 인식
online_correlative_scan_matching	true	로봇 위치가 튀어도 맞춰줌
linear_search_window	0.1	현재 추정 위치에서 전후좌우 0.1m 범위를 탐색
angular_search_window	20°	회전 시 좌우 20도 이내에서 가장 잘 맞는 각도를 탐색
min_score	0.55	라이다 데이터와 지도가 55% 이상 일치하면 아는 곳으로 판단
global_localization_min_score	0.6	완전히 길을 잃었을 때 자기 위치를 찾기 위한 최소 점수.

https://github.com/cartographer-project/cartographer_ros/tree/master/cartographer_ros/configuration_files

cartographer_ros/cartographer_ros/configuration_files at master · cartographer-project/cartographer_ros

carla_2D.lua 설정파일이 따로 있지 않고
기존 lua파일에서 수정했습니다.

3.

ros2 run cartographer_ros cartographer_occupancy_grid_node   --ros-args -p use_sim_time:=True -p resolution:=0.05

carla 기준 시간을 사용하기 위해 모두 --ros-args -p use_sim_time:=True 를 사용함

odom.py

carla에서 odometry토픽이 나오지 않아서 직접 publish

import carla
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion, TransformStamped
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
import tf2_ros
from rclpy.parameter import Parameter
import math

class CarlaGroundTruthOdometry(Node):
def __init__(self):
super().__init__('carla_gt_odometry_node')
# 시뮬레이션 시간 사용 강제
self.set_parameters([Parameter('use_sim_time', Parameter.Type.BOOL, True)])

qos_profile = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT, # 통신 효율 우선
history=HistoryPolicy.KEEP_LAST,
depth=10
)


try:
self.client = carla.Client('localhost', 2000)
self.client.set_timeout(10.0)
self.world = self.client.get_world()
self.get_logger().info("CARLA 서버에 연결되었습니다.")
except Exception as e:
self.get_logger().error(f"CARLA 연결 실패: {e}")
return

self.ego_vehicle = None
self.odom_pub = self.create_publisher(Odometry, '/carla/hero/odometry', 10)

# [중요] Cartographer와 충돌을 피하기 위해 TF 발행은 기본적으로 끕니다.
# self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)

self.odom_pub = self.create_publisher(Odometry, '/carla/hero/odometry', qos_profile)
self.timer = self.create_timer(0.02, self.publish_odometry)

def find_hero(self):
for actor in self.world.get_actors():
if actor.attributes.get('role_name') == 'hero':
self.ego_vehicle = actor
self.get_logger().info("Hero 차량을 찾았습니다!")
return True
return False

def publish_odometry(self):
if self.ego_vehicle is None:
if not self.find_hero(): return

try:
t = self.ego_vehicle.get_transform()
v = self.ego_vehicle.get_velocity()

# 1. 좌표 변환 (CARLA: 왼손 -> ROS: 오른손)
x, y, z = t.location.x, -t.location.y, t.location.z

# 2. 회전 변환
# CARLA(LHS) -> ROS(RHS) 변환을 위해 반드시 마이너스(-)를 붙여야 합니다.
roll = math.radians(t.rotation.roll)
pitch = math.radians(-t.rotation.pitch)
yaw = math.radians(-t.rotation.yaw) # 이 마이너스가 핵심입니다!

q = self.euler_to_quaternion(roll, pitch, yaw)
now = self.get_clock().now().to_msg()

# 3. Odometry 메시지 발행
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = 'odom'
odom.child_frame_id = 'hero'
odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z = x, y, z
odom.pose.pose.orientation = q

# 3. Odometry 메시지 발행 (속도 정보 Twist 추가)
cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw)
vx_ros, vy_ros = float(v.x), float(-v.y)

# [중요] 모든 할당값에 float()을 씌워 정수형(int) 유입을 원천 차단합니다.
odom.twist.twist.linear.x = float(vx_ros * cos_yaw + vy_ros * sin_yaw)
odom.twist.twist.linear.y = 0.0 # 반드시 0이 아닌 0.0 (실수형)이어야 합니다.
odom.twist.twist.linear.z = 0.0

av = self.ego_vehicle.get_angular_velocity()
odom.twist.twist.angular.x = 0.0
odom.twist.twist.angular.y = 0.0
odom.twist.twist.angular.z = float(math.radians(-av.z)) #

# 위치값도 혹시 모를 정수 유입을 방지합니다.
odom.pose.pose.position.x = float(x)
odom.pose.pose.position.y = float(y)
odom.pose.pose.position.z = float(z)

self.odom_pub.publish(odom)

# 4. TF 브로드캐스트는 주석 처리
# Cartographer LUA 설정에서 provide_odom_frame = true인 경우

# t_stamped = TransformStamped()
# t_stamped.header = odom.header
# t_stamped.child_frame_id = odom.child_frame_id
# t_stamped.transform.translation.x = x
# t_stamped.transform.translation.y = y
# t_stamped.transform.translation.z = z
# t_stamped.transform.rotation = q
# self.tf_broadcaster.sendTransform(t_stamped)


except Exception as e:
self.get_logger().error(f"Error: {e}")
self.ego_vehicle = None

def euler_to_quaternion(self, roll, pitch, yaw):
cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)
q = Quaternion()
q.w = cr * cp * cy + sr * sp * sy
q.x = sr * cp * cy - cr * sp * sy
q.y = cr * sp * cy + sr * cp * sy
q.z = cr * cp * sy - sr * sp * cy
return q

def main(args=None):
rclpy.init(args=args)
node = CarlaGroundTruthOdometry()
try:
rclpy.spin(node)
except KeyboardInterrupt: pass
finally:
node.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()

publish_odometry

t = self.ego_vehicle.get_transform()
v = self.ego_vehicle.get_velocity()


# 1. 좌표 변환 (CARLA: 왼손 -> ROS: 오른손)
x, y, z = t.location.x, -t.location.y, t.location.z

CARLA로부터 차량의 현재 위치/회전(transform)과 선속도(velocity)를 가져옴
Y축 반전: CARLA는 왼손 좌표계를 사용하므로, ROS 표준인 오른손 좌표계에 맞추기 위해 y 값에 마이너스

회전각 변환
roll = math.radians(t.rotation.roll)
pitch = math.radians(-t.rotation.pitch)
yaw = math.radians(-t.rotation.yaw) 
q= self.euler_to_quaternion(roll, pitch, yaw)


q
CARLA의 도(Degree) 단위를 ROS의 라디안(Radian)으로 바꿔줌
3개의 각도를 ROS가 이해하는 4차원 벡터(x,y,z,w)로 변환


변환된 x,−y,z 좌표와 쿼터니언 회전값을 메시지로 변환
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = 'odom'
odom.child_frame_id = 'hero'
odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z = x, y, z
odom.pose.pose.orientation = q

회전 변환
cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw)
vx_ros, vy_ros = float(v.x), float(-v.y)
odom.twist.twist.linear.x = float(vx_ros * cos_yaw + vy_ros * sin_yaw)
odom.twist.twist.linear.y = 0.0

(Twist)는 로봇 기준의 속도가 필요 하기 때문에 회전 행렬을 이용

주행할 경로

rviz2

ros2 run nav2_map_server map_saver_cli 맵저장

[Sungryul Lee]

코드는 깃허브에 저장하고 주소를 첨부할것, 이전과제보고서의 코드도 동일하게 깃허브에 저장할것
코드설명은 소스에서 핵심부분만 복사하여 설명할것
carla맵에서 어디의 지도를 작성한건지 캡쳐하여 첨부할것
carla_2D.lua 파일의 출처는?
오도메트리없이 한결과와 비교해볼것
carla맵과 slam 장면을 동시에 저장한 동영상을 만들어서 보내줄것