背景
在openscenario直接使用opendrive中的坐標(biāo)燎字，可以發(fā)現(xiàn)與carla地圖中的位置并不相對(duì)應(yīng)缘缚，因?yàn)閏arla使用的是左手坐標(biāo)系勾笆，而在opendrive和openscenario中使用的是右手坐標(biāo)系，因此在進(jìn)行相關(guān)的數(shù)據(jù)解析時(shí)需要進(jìn)行坐標(biāo)的轉(zhuǎn)換忙灼。下面摘抄出carla_scenario_runner中openscenario_parser中的坐標(biāo)轉(zhuǎn)換代碼以進(jìn)行查閱匠襟。
def set_use_carla_coordinate_system():
"""
CARLA internally uses a left-hand coordinate system (Unreal), but OpenSCENARIO and OpenDRIVE
are intended for right-hand coordinate system. Hence, we need to invert the coordinates, if
the scenario does not use CARLA coordinates, but instead right-hand coordinates.
"""
OpenScenarioParser.use_carla_coordinate_system = True
@staticmethod
def convert_position_to_transform(position, actor_list=None):
"""
Convert an OpenScenario position into a CARLA transform
Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently
does not provide sufficient access to OpenDrive information
Also not supported is Route. This can be added by checking additional
route information
"""
if position.find('WorldPosition') is not None:
world_pos = position.find('WorldPosition')
x = float(world_pos.attrib.get('x', 0))
y = float(world_pos.attrib.get('y', 0))
z = float(world_pos.attrib.get('z', 0))
yaw = math.degrees(float(world_pos.attrib.get('h', 0)))
pitch = math.degrees(float(world_pos.attrib.get('p', 0)))
roll = math.degrees(float(world_pos.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
y = y * (-1.0)
yaw = yaw * (-1.0)
return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
elif ((position.find('RelativeWorldPosition') is not None) or
(position.find('RelativeObjectPosition') is not None) or
(position.find('RelativeLanePosition') is not None)):
rel_pos = position.find('RelativeWorldPosition') or position.find(
'RelativeObjectPosition') or position.find('RelativeLanePosition')
# get relative object and relative position
obj = rel_pos.attrib.get('object')
obj_actor = None
actor_transform = None
if actor_list is not None:
for actor in actor_list:
if actor.rolename == obj:
obj_actor = actor
actor_transform = actor.transform
else:
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes['role_name'] == obj:
obj_actor = actor
actor_transform = obj_actor.get_transform()
break
if obj_actor is None:
raise AttributeError("Object '{}' provided as position reference is not known".format(obj))
# calculate orientation h, p, r
is_absolute = False
if rel_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
is_absolute = (orientation.attrib.get('type') == "absolute")
dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
droll = math.degrees(float(orientation.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
dyaw = dyaw * (-1.0)
yaw = actor_transform.rotation.yaw
pitch = actor_transform.rotation.pitch
roll = actor_transform.rotation.roll
if not is_absolute:
yaw = yaw + dyaw
pitch = pitch + dpitch
roll = roll + droll
else:
yaw = dyaw
pitch = dpitch
roll = droll
# calculate location x, y, z
# dx, dy, dz
if ((position.find('RelativeWorldPosition') is not None) or
(position.find('RelativeObjectPosition') is not None)):
dx = float(rel_pos.attrib.get('dx', 0))
dy = float(rel_pos.attrib.get('dy', 0))
dz = float(rel_pos.attrib.get('dz', 0))
if not OpenScenarioParser.use_carla_coordinate_system:
dy = dy * (-1.0)
x = actor_transform.location.x + dx
y = actor_transform.location.y + dy
z = actor_transform.location.z + dz
# dLane, ds, offset
elif position.find('RelativeLanePosition') is not None:
dlane = float(rel_pos.attrib.get('dLane'))
ds = float(rel_pos.attrib.get('ds'))
offset = float(rel_pos.attrib.get('offset'))
carla_map = CarlaDataProvider.get_map()
relative_waypoint = carla_map.get_waypoint(actor_transform.location)
if dlane == 0:
wp = relative_waypoint
elif dlane == -1:
wp = relative_waypoint.get_left_lane()
elif dlane == 1:
wp = relative_waypoint.get_right_lane()
if wp is None:
raise AttributeError("Object '{}' position with dLane={} is not valid".format(obj, dlane))
wp = wp.next(ds)[-1]
# Adapt transform according to offset
h = math.radians(wp.transform.rotation.yaw)
x_offset = math.sin(h) * offset
y_offset = math.cos(h) * offset
if OpenScenarioParser.use_carla_coordinate_system:
x_offset = x_offset * (-1.0)
y_offset = y_offset * (-1.0)
x = wp.transform.location.x + x_offset
y = wp.transform.location.y + y_offset
z = wp.transform.location.z
return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
# Not implemented
elif position.find('RoadPosition') is not None:
raise NotImplementedError("Road positions are not yet supported")
elif position.find('RelativeRoadPosition') is not None:
raise NotImplementedError("RelativeRoad positions are not yet supported")
elif position.find('LanePosition') is not None:
lane_pos = position.find('LanePosition')
road_id = int(lane_pos.attrib.get('roadId', 0))
lane_id = int(lane_pos.attrib.get('laneId', 0))
offset = float(lane_pos.attrib.get('offset', 0))
s = float(lane_pos.attrib.get('s', 0))
is_absolute = True
waypoint = CarlaDataProvider.get_map().get_waypoint_xodr(road_id, lane_id, s)
if waypoint is None:
raise AttributeError("Lane position cannot be found")
transform = waypoint.transform
if lane_pos.find('Orientation') is not None:
orientation = rel_pos.find('Orientation')
dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
droll = math.degrees(float(orientation.attrib.get('r', 0)))
if not OpenScenarioParser.use_carla_coordinate_system:
dyaw = dyaw * (-1.0)
transform.rotation.yaw = transform.rotation.yaw + dyaw
transform.rotation.pitch = transform.rotation.pitch + dpitch
transform.rotation.roll = transform.rotation.roll + droll
if offset != 0:
forward_vector = transform.rotation.get_forward_vector()
orthogonal_vector = carla.Vector3D(x=-forward_vector.y, y=forward_vector.x, z=forward_vector.z)
transform.location.x = transform.location.x + offset * orthogonal_vector.x
transform.location.y = transform.location.y + offset * orthogonal_vector.y
return transform
elif position.find('RoutePosition') is not None:
raise NotImplementedError("Route positions are not yet supported")
else:
raise AttributeError("Unknown position")
CARLA中的坐標(biāo)系
OpenDrive中的坐標(biāo)系
