跳过主要内容

FOT( Frenet Optimal Planner, Frenet 最优规划师) 算子

FOT( Frenet Optimal Planner, Frenet 最优规划师) 算子,基于 https://github.com/erdos-project/frenet_optimal_trajectory_planner/ 并包装不同的元素障碍物位置速度 ...转换为 Frenet 消费格式。

FOT 输入为:

initial_conditions = {
"ps": 0,
"target_speed": # 目标速度
"pos": # x, y 当前位置
"vel": # vx, vy 当前速度
"wp": # [[x, y], ... n_waypoints ] 所需航点
"obs": # [[min_x, min_y, max_x, max_y], ... ] 路上障碍
}

下面还有一组超参数。

由于我们的障碍物被定义为 3D 点,我们需要将这些点转换为 [min_x, min_y, max_x, max_y] 格式。 我们在 get_obstacle_list 函数中执行此操作。 这种近似值非常基本,可能需要重新审视。

输出要么是成功的轨迹,我们可以将其输入到 PID 中。 或者它是失败的,在这种情况下,我们将当前位置作为航点发送。

输入

  • 图像: 高 x 宽 x BGR array。

输出

  • 航点:x_points, y_points, 速度

示例图像

Imgur

图描述

  - id: fot_op
operator:
python: ../../operators/fot_op.py
outputs:
- waypoints
inputs:
position: oasis_agent/position
speed: oasis_agent/speed
obstacles: obstacle_location_op/obstacles
gps_waypoints: carla_gps_op/gps_waypoints

图可视化

方法

__init__()

源码
    def __init__(self):
self.obstacles = np.array([])
self.lanes = np.array([])
self.position = []
self.speed = []
self.last_position = []
self.waypoints = []
self.gps_waypoints = np.array([])
self.last_obstacles = np.array([])
self.obstacle_metadata = {}
self.gps_metadata = {}
self.metadata = {}
self.orientation = None
self.outputs = []
self.hyperparameters = {
"max_speed": 25.0,
"max_accel": 45.0,
"max_curvature": 55.0,
"max_road_width_l": 0.1,
"max_road_width_r": 0.1,
"d_road_w": 0.5,
"dt": 0.5,
"maxt": 5.0,
"mint": 2.0,
"d_t_s": 5,
"n_s_sample": 2.0,
"obstacle_clearance": 0.1,
"kd": 1.0,
"kv": 0.1,
"ka": 0.1,
"kj": 0.1,
"kt": 0.1,
"ko": 0.1,
"klat": 1.0,
"klon": 1.0,
"num_threads": 0, # 设置 0 以避免使用线程算法
}
self.conds = {
"s0": 0,
"target_speed": TARGET_SPEED,
} # 粘贴调试日志的输出


.on_event(...)

源码

def on_event(
self,
dora_event: dict,
send_output: Callable[[str, bytes], None],
) -> DoraStatus:
if dora_event["type"] == "INPUT":
return self.on_input(dora_event, send_output)
return DoraStatus.CONTINUE


.on_input(...)

源码

def on_input(
self,
dora_input: dict,
send_output: Callable[[str, bytes], None],
):
if dora_input["id"] == "position":
self.last_position = self.position
self.position = np.array(dora_input["value"])
if len(self.last_position) == 0:
self.last_position = self.position
return DoraStatus.CONTINUE

elif dora_input["id"] == "speed":
self.speed = np.array(dora_input["value"])
return DoraStatus.CONTINUE

elif dora_input["id"] == "obstacles":
obstacles = np.array(dora_input["value"]).reshape((-1, 5))
if len(self.last_obstacles) > 0:
self.obstacles = np.concatenate([self.last_obstacles, obstacles])
else:
self.obstacles = obstacles

elif dora_input["id"] == "global_lanes":
lanes = np.array(dora_input["value"]).reshape((-1, 60, 3))
self.lanes = lanes
return DoraStatus.CONTINUE

elif "gps_waypoints" == dora_input["id"]:
waypoints = np.array(dora_input["value"])
waypoints = waypoints.reshape((-1, 3))[:, :2]
self.gps_waypoints = waypoints
return DoraStatus.CONTINUE

if len(self.gps_waypoints) == 0:
print("No waypoints")
send_output(
"waypoints",
self.gps_waypoints.tobytes(),
dora_input["metadata"],
)
return DoraStatus.CONTINUE

elif len(self.position) == 0 or len(self.speed) == 0:
return DoraStatus.CONTINUE

[x, y, z, rx, ry, rz, rw] = self.position
[_, _, yaw] = R.from_quat([rx, ry, rz, rw]).as_euler("xyz", degrees=False)

gps_obstacles = get_obstacle_list(
self.position, self.obstacles, self.gps_waypoints
)

if len(self.lanes) > 0:
lanes = get_lane_list(self.position, self.lanes, self.gps_waypoints)
obstacles = np.concatenate([gps_obstacles, lanes])
else:
obstacles = gps_obstacles
initial_conditions = {
"ps": 0,
"target_speed": self.conds["target_speed"],
"pos": self.position[:2],
"vel": (np.clip(LA.norm(self.speed), 0.5, 40))
* np.array([np.cos(yaw), np.sin(yaw)]),
"wp": self.gps_waypoints,
"obs": obstacles,
}

(
result_x,
result_y,
speeds,
ix,
iy,
iyaw,
d,
s,
speeds_x,
speeds_y,
misc,
costs,
success,
) = fot_wrapper.run_fot(initial_conditions, self.hyperparameters)

if not success:
initial_conditions["wp"] = initial_conditions["wp"][:5]
print(f"fot failed. stopping with {initial_conditions}.")
target_distance = LA.norm(self.gps_waypoints[-1] - self.position[:2])
print(f"Distance to target: {target_distance}")
for obstacle in self.obstacles:
print(f"obstacles:{obstacle}, label: {LABELS[int(obstacle[-1])]}")

send_output(
"waypoints",
pa.array(np.array([x, y, 0.0], np.float32)),
dora_input["metadata"],
)
return DoraStatus.CONTINUE

self.waypoints = np.concatenate([result_x, result_y]).reshape((2, -1)).T

self.outputs = np.ascontiguousarray(
np.concatenate([result_x, result_y, speeds])
.reshape((3, -1))
.T.astype(np.float32)
)
send_output(
"waypoints",
pa.array(self.outputs.ravel()),
dora_input["metadata"],
)
return DoraStatus.CONTINUE