背景
在openscenario直接使用opendrive中的坐标，可以发现与carla地图中的位置并不相对应，因为carla使用的是左手坐标系，而在opendrive和openscenario中使用的是右手坐标系，因此在进行相关的数据解析时需要进行坐标的转换。下面摘抄出carla_scenario_runner中openscenario_parser中的坐标转换代码以进行查阅。
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中的坐标系
OpenDrive中的坐标系
网友评论