mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
Jimmy
@@ -91,14 +91,6 @@ tools/replay/replay <route-name>
|
||||
cd selfdrive/ui && ./ui
|
||||
```
|
||||
|
||||
## Try Radar Point Visualization with Rerun
|
||||
To visualize radar points, run rp_visualization.py while tools/replay/replay is active.
|
||||
|
||||
```bash
|
||||
tools/replay/replay <route-name>
|
||||
python3 replay/rp_visualization.py
|
||||
```
|
||||
|
||||
## Work with plotjuggler
|
||||
If you want to use replay with plotjuggler, you can stream messages by running:
|
||||
|
||||
|
||||
@@ -1,109 +0,0 @@
|
||||
import numpy as np
|
||||
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
|
||||
|
||||
# Color palette used for rerun AnnotationContext
|
||||
rerunColorPalette = [(96, "red", (255, 0, 0)),
|
||||
(100, "pink", (255, 36, 0)),
|
||||
(124, "yellow", (255, 255, 0)),
|
||||
(230, "vibrantpink", (255, 36, 170)),
|
||||
(240, "orange", (255, 146, 0)),
|
||||
(255, "white", (255, 255, 255)),
|
||||
(110, "carColor", (255,0,127)),
|
||||
(0, "background", (0, 0, 0))]
|
||||
|
||||
|
||||
class UIParams:
|
||||
lidar_x, lidar_y, lidar_zoom = 384, 960, 6
|
||||
lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
|
||||
car_hwidth = 1.7272 / 2 * lidar_zoom
|
||||
car_front = 2.6924 * lidar_zoom
|
||||
car_back = 1.8796 * lidar_zoom
|
||||
car_color = rerunColorPalette[6][0]
|
||||
UP = UIParams
|
||||
|
||||
|
||||
def to_topdown_pt(y, x):
|
||||
px, py = x * UP.lidar_zoom + UP.lidar_car_x, -y * UP.lidar_zoom + UP.lidar_car_y
|
||||
if px > 0 and py > 0 and px < UP.lidar_x and py < UP.lidar_y:
|
||||
return int(px), int(py)
|
||||
return -1, -1
|
||||
|
||||
|
||||
def draw_path(path, lid_overlay, lid_color=None):
|
||||
x, y = np.asarray(path.x), np.asarray(path.y)
|
||||
# draw lidar path point on lidar
|
||||
if lid_color is not None and lid_overlay is not None:
|
||||
for i in range(len(x)):
|
||||
px, py = to_topdown_pt(x[i], y[i])
|
||||
if px != -1:
|
||||
lid_overlay[px, py] = lid_color
|
||||
|
||||
|
||||
def plot_model(m, lid_overlay):
|
||||
if lid_overlay is None:
|
||||
return
|
||||
for lead in m.leadsV3:
|
||||
if lead.prob < 0.5:
|
||||
continue
|
||||
x, y = lead.x[0], lead.y[0]
|
||||
x_std = lead.xStd[0]
|
||||
x -= RADAR_TO_CAMERA
|
||||
_, py_top = to_topdown_pt(x + x_std, y)
|
||||
px, py_bottom = to_topdown_pt(x - x_std, y)
|
||||
lid_overlay[int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = rerunColorPalette[2][0]
|
||||
|
||||
for path in m.laneLines:
|
||||
draw_path(path, lid_overlay, rerunColorPalette[2][0])
|
||||
for edge in m.roadEdges:
|
||||
draw_path(edge, lid_overlay, rerunColorPalette[0][0])
|
||||
draw_path(m.position, lid_overlay, rerunColorPalette[0][0])
|
||||
|
||||
|
||||
def plot_lead(rs, lid_overlay):
|
||||
for lead in [rs.leadOne, rs.leadTwo]:
|
||||
if not lead.status:
|
||||
continue
|
||||
x = lead.dRel
|
||||
px_left, py = to_topdown_pt(x, -10)
|
||||
px_right, _ = to_topdown_pt(x, 10)
|
||||
lid_overlay[px_left:px_right, py] = rerunColorPalette[0][0]
|
||||
|
||||
|
||||
def update_radar_points(lt, lid_overlay):
|
||||
ar_pts = []
|
||||
if lt is not None:
|
||||
ar_pts = {}
|
||||
for track in lt:
|
||||
ar_pts[track.trackId] = [track.dRel, track.yRel, track.vRel, track.aRel, track.oncoming, track.stationary]
|
||||
for ids, pt in ar_pts.items():
|
||||
# negative here since radar is left positive
|
||||
px, py = to_topdown_pt(pt[0], -pt[1])
|
||||
if px != -1:
|
||||
if pt[-1]:
|
||||
color = rerunColorPalette[4][0]
|
||||
elif pt[-2]:
|
||||
color = rerunColorPalette[3][0]
|
||||
else:
|
||||
color = rerunColorPalette[5][0]
|
||||
if int(ids) == 1:
|
||||
lid_overlay[px - 2:px + 2, py - 10:py + 10] = rerunColorPalette[1][0]
|
||||
else:
|
||||
lid_overlay[px - 2:px + 2, py - 2:py + 2] = color
|
||||
|
||||
|
||||
def get_blank_lid_overlay(UP):
|
||||
lid_overlay = np.zeros((UP.lidar_x, UP.lidar_y), 'uint8')
|
||||
# Draw the car.
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y -
|
||||
UP.car_front))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y +
|
||||
UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x + UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
return lid_overlay
|
||||
@@ -1,60 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import numpy as np
|
||||
import rerun as rr
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.tools.replay.lib.rp_helpers import (UP, rerunColorPalette,
|
||||
get_blank_lid_overlay,
|
||||
update_radar_points, plot_lead,
|
||||
plot_model)
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
UP.lidar_zoom = 6
|
||||
|
||||
def visualize(addr):
|
||||
sm = messaging.SubMaster(['radarState', 'liveTracks', 'modelV2'], addr=addr)
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
while True:
|
||||
if not vipc_client.is_connected():
|
||||
vipc_client.connect(True)
|
||||
new_data = vipc_client.recv()
|
||||
if new_data is None or not new_data.data.any():
|
||||
continue
|
||||
|
||||
sm.update(0)
|
||||
lid_overlay = get_blank_lid_overlay(UP)
|
||||
if sm.recv_frame['modelV2']:
|
||||
plot_model(sm['modelV2'], lid_overlay)
|
||||
if sm.recv_frame['radarState']:
|
||||
plot_lead(sm['radarState'], lid_overlay)
|
||||
liveTracksTime = sm.logMonoTime['liveTracks']
|
||||
if sm.updated['liveTracks']:
|
||||
update_radar_points(sm['liveTracks'], lid_overlay)
|
||||
rr.set_time_nanos("TIMELINE", liveTracksTime)
|
||||
rr.log("tracks", rr.SegmentationImage(np.flip(np.rot90(lid_overlay, k=-1), axis=1)))
|
||||
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Show replay data in a UI.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1",
|
||||
help="The ip address on which to receive zmq messages.")
|
||||
parser.add_argument("--frame-address", default=None,
|
||||
help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
if args.ip_address != "127.0.0.1":
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
rr.init("RadarPoints", spawn= True)
|
||||
rr.log("tracks", rr.AnnotationContext(rerunColorPalette), static=True)
|
||||
visualize(args.ip_address)
|
||||
Reference in New Issue
Block a user