[Question]: NavDp模型部署宇树g1问题

[Summerflyingg]

Question

hello,您好！请问在实际将NavDp模型部署在宇树g1上时，出现本来朝着目标方向走，但是后面走一段时间之后，就会掉转方向朝着别的地方走。这种现象可能是由于什么原因呢？以及在部署到真机的时候，我需要注意哪些点来确保实际效果接近论文中所提及的效果呢？感谢感谢

[yuqiang-yang]

请问是NavDP还是InternVLA-N1-DualVLN呢，NavDP只是一个导航避障S1模型

[Summerflyingg]

sorry 应该是InternRobotics/InternVLA-N1-w-NavDP这个模型

[Summerflyingg]

sorry 应该是InternRobotics/InternVLA-N1-w-NavDP这个模型

您好 其实我同时在给g1上测试了 这两个模型InternRobotics/InternVLA-N1-w-NavDP和InternVLA-N1-DualVLN，但是最终效果来看的话。N1-w-NavDp的效果略好于N1-DualVLN。N1-w-NavDp 我遇到的情况就是“本来朝着目标方向走，但是后面走一段时间之后，就会掉转方向朝着别的地方走”，但是“N1-DualVLN”的话，就是完全找不到目标，或者朝着错误的方向走。
我参考您所release出的internvla_client写了g1的client端代码
同时也检查了server端 对齐了训练时的参数 384*384 以及俯角15° 但不知道是否还是忽略掉某些问题
import argparse
import copy
import json
import math
import os
import pickle
import threading
import time
from collections import deque
from enum import Enum

import cv2
import numpy as np
import requests
import zmq

try:
import rospy
from nav_msgs.msg import Odometry
except ImportError: # pragma: no cover - depends on robot runtime
rospy = None
Odometry = None

try:
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
except ImportError: # pragma: no cover - depends on robot runtime
ChannelFactoryInitialize = None
LocoClient = None

from controllers import Mpc_controller, PID_controller
from thread_utils import ReadWriteLock

class ControlMode(Enum):
PID_MODE = 1
MPC_MODE = 2

class G1Controller:
def init(
self,
network_interface: str = "eth0",
dry_run: bool = True,
call_start: bool = False,
loco_timeout: float = 10.0,
keepalive_period: float = 0.1,
):
self.dry_run = dry_run
self.call_start = call_start
self.ready = False
self.loco = None
self.last_continuous_cmd = None
self.target_continuous_cmd = (0.0, 0.0)
self.keepalive_period = keepalive_period
self.keepalive_running = False
self.keepalive_thread = None
self.cmd_lock = threading.Lock()

    if self.dry_run:
        print("[ctrl] dry-run mode, no motion commands will be sent")
        return

    if ChannelFactoryInitialize is None or LocoClient is None:
        raise RuntimeError("unitree_sdk2py is required for live mode but is not importable")

    ChannelFactoryInitialize(0, network_interface)
    self.loco = LocoClient()
    self.loco.SetTimeout(loco_timeout)
    self.loco.Init()
    print(f"[ctrl] LocoClient initialized on interface={network_interface}")

    if self.call_start:
        print("[ctrl] calling Start() once")
        self.loco.Start()
        time.sleep(0.5)

    self.ready = True
    self.keepalive_running = True
    self.keepalive_thread = threading.Thread(target=self._keepalive_loop, daemon=True)
    self.keepalive_thread.start()

def _keepalive_loop(self):
    while self.keepalive_running:
        with self.cmd_lock:
            vx, wz = self.target_continuous_cmd
        if self.ready and (abs(vx) > 1e-6 or abs(wz) > 1e-6):
            try:
                self.loco.Move(float(vx), 0.0, float(wz), continous_move=True)
            except Exception as exc:  # pragma: no cover - robot runtime
                print("[ctrl] keepalive move failed:", exc)
        time.sleep(self.keepalive_period)

def set_continuous_command(self, vx: float, wz: float, label: str):
    cmd = (float(vx), float(wz))
    if self.last_continuous_cmd != cmd:
        print(f"[ctrl] {label} vx={vx:.2f} wz={wz:.2f}")
    with self.cmd_lock:
        self.target_continuous_cmd = cmd
    if self.ready:
        self.loco.Move(float(vx), 0.0, float(wz), continous_move=True)
    self.last_continuous_cmd = cmd

def stop(self):
    if self.last_continuous_cmd != (0.0, 0.0):
        print("[ctrl] STOP")
    with self.cmd_lock:
        self.target_continuous_cmd = (0.0, 0.0)
    if self.ready:
        try:
            self.loco.StopMove()
        except Exception as exc:  # pragma: no cover - robot runtime
            print("[ctrl] stop failed:", exc)
    self.last_continuous_cmd = (0.0, 0.0)

def close(self):
    self.keepalive_running = False
    if self.keepalive_thread is not None:
        self.keepalive_thread.join(timeout=0.5)

class ZmqSyncReceiver:
def init(self, zmq_url: str, recv_timeout_ms: int = 500, conflate: bool = True):
self.ctx = zmq.Context.instance()
self.sock = self.ctx.socket(zmq.SUB)
if conflate and hasattr(zmq, "CONFLATE"):
self.sock.setsockopt(zmq.CONFLATE, 1)
self.sock.setsockopt(zmq.SUBSCRIBE, b"")
if recv_timeout_ms > 0:
self.sock.setsockopt(zmq.RCVTIMEO, recv_timeout_ms)
self.sock.connect(zmq_url)
time.sleep(0.05)

def recv_latest_packet(self) -> bytes:
    data = self.sock.recv()
    while True:
        try:
            data = self.sock.recv(flags=zmq.NOBLOCK)
        except zmq.Again:
            break
    return data

def close(self):
    self.sock.close()

def decode_sync_packet(packet: bytes):
payload = pickle.loads(packet)
if not isinstance(payload, dict):
raise RuntimeError(f"expected dict sync payload, got {type(payload)!r}")

meta = payload.get("meta")
rgb_jpg = payload.get("rgb_jpg")
depth_png = payload.get("depth_png")
if meta is None or rgb_jpg is None or depth_png is None:
    raise RuntimeError("sync payload missing meta/rgb_jpg/depth_png")

rgb_image = cv2.imdecode(np.frombuffer(rgb_jpg, dtype=np.uint8), cv2.IMREAD_COLOR)
depth_u16 = cv2.imdecode(np.frombuffer(depth_png, dtype=np.uint8), cv2.IMREAD_UNCHANGED)
if rgb_image is None:
    raise RuntimeError("failed to decode RGB jpg from sync packet")
if depth_u16 is None:
    raise RuntimeError("failed to decode depth png from sync packet")
if depth_u16.dtype != np.uint16:
    raise RuntimeError(f"expected uint16 depth, got {depth_u16.dtype}")
return meta, rgb_jpg, depth_png, rgb_image, depth_u16

def raw_depth_to_meters(depth_u16: np.ndarray, depth_scale: float) -> np.ndarray:
depth_m = depth_u16.astype(np.float32) * float(depth_scale)
depth_m[np.isnan(depth_m)] = 0.0
depth_m[np.isinf(depth_m)] = 0.0
depth_m[depth_m < 0.0] = 0.0
return depth_m

def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float:
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
return math.atan2(siny_cosp, cosy_cosp)

def build_homo_pose(x: float, y: float, yaw: float) -> np.ndarray:
pose = np.eye(4)
pose[0, 0] = math.cos(yaw)
pose[0, 1] = -math.sin(yaw)
pose[1, 0] = math.sin(yaw)
pose[1, 1] = math.cos(yaw)
pose[0, 3] = x
pose[1, 3] = y
return pose

def apply_motion_deadband(vx: float, wz: float, min_forward_speed: float, min_turn_speed: float):
clamped_vx = float(vx)
clamped_wz = float(wz)

if abs(clamped_vx) > 1e-6 and abs(clamped_vx) < min_forward_speed:
    clamped_vx = math.copysign(min_forward_speed, clamped_vx)

if abs(clamped_wz) > 1e-6 and abs(clamped_wz) < min_turn_speed:
    clamped_wz = math.copysign(min_turn_speed, clamped_wz)

changed = abs(clamped_vx - vx) > 1e-6 or abs(clamped_wz - wz) > 1e-6
return clamped_vx, clamped_wz, changed

def post_to_server(server_url: str, rgb_jpg: bytes, depth_png: bytes, instruction: str, reset: bool, req_idx: int) -> dict:
payload = {
"reset": reset,
"idx": req_idx,
"instruction": instruction,
}
files = {
"image": ("rgb.jpg", rgb_jpg, "image/jpeg"),
}
if depth_png is not None:
files["depth"] = ("depth.png", depth_png, "image/png")

response = requests.post(
    server_url,
    files=files,
    data={"json": json.dumps(payload)},
    timeout=100,
)
response.raise_for_status()
return response.json()

class Ros1OdomReceiver:
def init(self, topic: str, queue_size: int = 100):
if rospy is None or Odometry is None:
raise RuntimeError("ROS1 odom support requires rospy and nav_msgs")
if not rospy.core.is_initialized():
rospy.init_node("g1_dual_bridge_stream", anonymous=True, disable_signals=True)

    self.lock = threading.Lock()
    self.latest_pose = None
    self.latest_homo = None
    self.latest_vel = None
    self.queue = deque(maxlen=queue_size)
    self.sub = rospy.Subscriber(topic, Odometry, self._callback, queue_size=10)

def _callback(self, msg):
    yaw = quaternion_to_yaw(
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w,
    )
    pose_list = [float(msg.pose.pose.position.x), float(msg.pose.pose.position.y), float(yaw)]
    stamp = time.time()
    homo = build_homo_pose(pose_list[0], pose_list[1], pose_list[2])
    vel = [float(msg.twist.twist.linear.x), float(msg.twist.twist.angular.z)]
    with self.lock:
        self.latest_pose = pose_list
        self.latest_homo = homo
        self.latest_vel = vel
        self.queue.append((stamp, copy.deepcopy(pose_list)))

def get_latest_state(self):
    with self.lock:
        pose = copy.deepcopy(self.latest_pose) if self.latest_pose is not None else None
        homo = self.latest_homo.copy() if self.latest_homo is not None else None
        vel = copy.deepcopy(self.latest_vel) if self.latest_vel is not None else None
        return pose, homo, vel

def get_nearest_pose(self, target_time: float):
    with self.lock:
        if not self.queue:
            return None
        nearest_pose = None
        min_diff = 1e10
        for stamp, pose in self.queue:
            diff = abs(stamp - target_time)
            if diff < min_diff:
                min_diff = diff
                nearest_pose = copy.deepcopy(pose)
        return nearest_pose

def close(self):
    if self.sub is not None:
        self.sub.unregister()

class G1BridgeState:
def init(self, pid: PID_controller):
self.pid = pid
self.reset = True
self.http_idx = -1
self.rgb_depth_rw_lock = ReadWriteLock()
self.odom_rw_lock = ReadWriteLock()
self.mpc_rw_lock = ReadWriteLock()
self.current_control_mode = ControlMode.MPC_MODE
self.rgb_jpg = None
self.depth_png = None
self.rgb_image = None
self.depth_image = None
self.rgb_time = 0.0
self.frame_meta = None
self.homo_goal = None
self.homo_odom = None
self.vel = None
self.odom = None
self.last_trajs_in_world = None
self.request_cnt = 0
self.mpc = None

def update_frame(self, meta, rgb_jpg, depth_png, rgb_image, depth_image):
    self.rgb_depth_rw_lock.acquire_write()
    self.frame_meta = copy.deepcopy(meta)
    self.rgb_jpg = rgb_jpg
    self.depth_png = depth_png
    self.rgb_image = rgb_image.copy()
    self.depth_image = depth_image.copy()
    self.rgb_time = float(meta.get("timestamp", time.time()))
    self.rgb_depth_rw_lock.release_write()

def update_odom_state(self, odom_pose, homo_odom, vel):
    self.odom_rw_lock.acquire_write()
    self.odom = copy.deepcopy(odom_pose)
    self.homo_odom = homo_odom.copy() if homo_odom is not None else None
    self.vel = copy.deepcopy(vel) if vel is not None else None
    if self.homo_goal is None and self.homo_odom is not None:
        self.homo_goal = self.homo_odom.copy()
    self.odom_rw_lock.release_write()

def incremental_change_goal(self, actions):
    if self.homo_odom is None:
        raise ValueError("Please initialize homo_odom before changing goal")
    homo_goal = self.homo_odom.copy()
    for each_action in actions:
        if each_action == 0:
            pass
        elif each_action == 1:
            yaw = math.atan2(homo_goal[1, 0], homo_goal[0, 0])
            homo_goal[0, 3] += 0.25 * math.cos(yaw)
            homo_goal[1, 3] += 0.25 * math.sin(yaw)
        elif each_action == 2:
            angle = math.radians(15.0)
            rotation_matrix = np.array(
                [[math.cos(angle), -math.sin(angle), 0], [math.sin(angle), math.cos(angle), 0], [0, 0, 1]]
            )
            homo_goal[:3, :3] = np.dot(rotation_matrix, homo_goal[:3, :3])
        elif each_action == 3:
            angle = -math.radians(15.0)
            rotation_matrix = np.array(
                [[math.cos(angle), -math.sin(angle), 0], [math.sin(angle), math.cos(angle), 0], [0, 0, 1]]
            )
            homo_goal[:3, :3] = np.dot(rotation_matrix, homo_goal[:3, :3])
    self.homo_goal = homo_goal

def save_packet_bytes(run_dir: str, req_idx: int, reset_flag: bool, rgb_jpg: bytes, depth_png: bytes):
rgb_dir = os.path.join(run_dir, "rgb")
depth_dir = os.path.join(run_dir, "depth")
os.makedirs(rgb_dir, exist_ok=True)
os.makedirs(depth_dir, exist_ok=True)
stem = f"req_{req_idx:06d}_reset{int(reset_flag)}"
with open(os.path.join(rgb_dir, stem + ".jpg"), "wb") as file_obj:
file_obj.write(rgb_jpg)
with open(os.path.join(depth_dir, stem + ".png"), "wb") as file_obj:
file_obj.write(depth_png)

def build_run_dir(base_dir: str, run_name: str):
if run_name:
run_dir = os.path.join(base_dir, run_name)
else:
run_dir = os.path.join(base_dir, time.strftime("run_%Y%m%d_%H%M%S"))
os.makedirs(run_dir, exist_ok=True)
return run_dir

def planning_thread_fn(state: G1BridgeState, odom_receiver: Ros1OdomReceiver, receiver: ZmqSyncReceiver, args):
while True:
start_time = time.time()
try:
packet = receiver.recv_latest_packet()
meta, rgb_jpg, depth_png, rgb_image, depth_u16 = decode_sync_packet(packet)
depth_scale = float(meta.get("depth_scale") or args.depth_scale)
depth_m = raw_depth_to_meters(depth_u16, depth_scale)
state.update_frame(meta, rgb_jpg, depth_png, rgb_image, depth_m)

        pose, homo_odom, vel = odom_receiver.get_latest_state()
        if pose is not None and homo_odom is not None and vel is not None:
            state.update_odom_state(pose, homo_odom, vel)

        odom_infer = odom_receiver.get_nearest_pose(state.rgb_time)
        if odom_infer is None:
            print("[plan] skip planning: no odom available")
            time.sleep(0.1)
            continue

        req_idx = state.http_idx
        reset_flag = state.reset
        if args.save_frames:
            save_packet_bytes(args.run_dir, req_idx + 1, reset_flag, rgb_jpg, depth_png)

        response = post_to_server(
            args.server_url,
            rgb_jpg,
            depth_png,
            args.instruction,
            state.reset,
            state.http_idx,
        )
        state.reset = False
        state.http_idx += 1
        print(f"[plan] response idx={state.http_idx}: {json.dumps(response, ensure_ascii=False)}")

        if "trajectory" in response:
            trajectory = list(response["trajectory"])
            trajs_in_world = []
            x_, y_, yaw_ = odom_infer[0], odom_infer[1], odom_infer[2]
            w_T_b = np.array(
                [
                    [math.cos(yaw_), -math.sin(yaw_), 0, x_],
                    [math.sin(yaw_), math.cos(yaw_), 0, y_],
                    [0.0, 0.0, 1.0, 0.0],
                    [0.0, 0.0, 0.0, 1.0],
                ]
            )
            for i, traj in enumerate(trajectory):
                if i < args.trajectory_skip:
                    continue
                w_P = (w_T_b @ np.array([traj[0], traj[1], 0.0, 1.0]).T)[:2]
                trajs_in_world.append(w_P)

            if not trajs_in_world:
                print("[plan] empty world trajectory after skip, keep last controller state")
            else:
                trajs_in_world = np.array(trajs_in_world)
                state.mpc_rw_lock.acquire_write()
                if state.mpc is None:
                    state.mpc = Mpc_controller(np.array(trajs_in_world))
                else:
                    state.mpc.update_ref_traj(np.array(trajs_in_world))
                state.last_trajs_in_world = trajs_in_world
                state.request_cnt += 1
                state.current_control_mode = ControlMode.MPC_MODE
                state.mpc_rw_lock.release_write()
                print(f"[plan] updated world trajectory with {len(trajs_in_world)} points")
        elif "discrete_action" in response:
            actions = list(response["discrete_action"])
            if actions not in ([5], [9]):
                state.odom_rw_lock.acquire_write()
                try:
                    state.incremental_change_goal(actions)
                    state.current_control_mode = ControlMode.PID_MODE
                finally:
                    state.odom_rw_lock.release_write()
                print(f"[plan] updated incremental goal from actions {actions}")
        else:
            print("[plan] response has no trajectory/discrete_action")
    except zmq.Again:
        print("[plan] waiting for sync packet from stream input")
    except Exception as exc:
        print("[plan] failed:", exc)

    elapsed = time.time() - start_time
    time.sleep(max(0.0, args.plan_period - elapsed))

def control_thread_fn(state: G1BridgeState, controller: G1Controller, args):
while True:
try:
state.odom_rw_lock.acquire_read()
odom = copy.deepcopy(state.odom) if state.odom is not None else None
homo_odom = state.homo_odom.copy() if state.homo_odom is not None else None
vel = copy.deepcopy(state.vel) if state.vel is not None else None
homo_goal = state.homo_goal.copy() if state.homo_goal is not None else None
current_mode = state.current_control_mode
state.odom_rw_lock.release_read()

        if current_mode == ControlMode.MPC_MODE:
            state.mpc_rw_lock.acquire_read()
            local_mpc = state.mpc
            state.mpc_rw_lock.release_read()
            if local_mpc is not None and odom is not None:
                opt_u_controls, _ = local_mpc.solve(np.array(odom))
                v = float(opt_u_controls[0, 0])
                w = float(opt_u_controls[0, 1])
                orig_v, orig_w = v, w
                v, w, clamped = apply_motion_deadband(v, w, args.min_forward_speed, args.min_turn_speed)
                if clamped:
                    print(f"[ctrl] MPC deadband clamp: vx {orig_v:.2f}->{v:.2f}, wz {orig_w:.2f}->{w:.2f}")
                controller.set_continuous_command(v, w, "MPC")
        elif current_mode == ControlMode.PID_MODE:
            if homo_odom is not None and vel is not None and homo_goal is not None:
                v, w, e_p, e_r = state.pid.solve(homo_odom, homo_goal, vel)
                if v < 0.0:
                    v = 0.0
                orig_v, orig_w = v, w
                v, w, clamped = apply_motion_deadband(v, w, args.min_forward_speed, args.min_turn_speed)
                if clamped:
                    print(f"[ctrl] PID deadband clamp: vx {orig_v:.2f}->{v:.2f}, wz {orig_w:.2f}->{w:.2f}")
                controller.set_continuous_command(v, w, f"PID e_p={e_p:.2f} e_r={e_r:.2f}")
    except Exception as exc:
        print("[ctrl] control loop failed:", exc)

    time.sleep(args.control_period)

def parse_args():
parser = argparse.ArgumentParser(description="Stream-mode G1 bridge for InternNav DualVLN server.")
parser.add_argument("--rgbd-sync-url", type=str, default="tcp://127.0.0.1:5557")
parser.add_argument("--recv-timeout-ms", type=int, default=500)
parser.add_argument("--no-conflate", action="store_true", help="Disable ZMQ CONFLATE on the sync subscriber.")
parser.add_argument("--server-url", type=str, required=True)
parser.add_argument("--instruction", type=str, required=True)
parser.add_argument("--odom-topic", type=str, default="/fastlio_odom")
parser.add_argument("--network-interface", type=str, default="eth0")
parser.add_argument("--plan-period", type=float, default=0.30)
parser.add_argument("--control-period", type=float, default=0.10)
parser.add_argument("--trajectory-skip", type=int, default=3)
parser.add_argument("--min-forward-speed", type=float, default=0.25)
parser.add_argument("--min-turn-speed", type=float, default=0.30)
parser.add_argument("--depth-scale", type=float, default=0.001)
parser.add_argument("--loco-timeout", type=float, default=10.0)
parser.add_argument("--call-start", action="store_true", help="Call LocoClient.Start() once at startup.")
parser.set_defaults(dry_run=True)
parser.add_argument("--dry-run", dest="dry_run", action="store_true")
parser.add_argument("--live", dest="dry_run", action="store_false")
parser.add_argument("--save-frames", action="store_true")
parser.add_argument("--frames-dir", type=str, default="sent_frames_stream_style")
parser.add_argument("--run-name", type=str, default="")
return parser.parse_args()

def main():
args = parse_args()

args.run_dir = build_run_dir(args.frames_dir, args.run_name) if args.save_frames else None
if args.save_frames:
    print(f"[run] saving frames to {os.path.abspath(args.run_dir)}")

pid = PID_controller(Kp_trans=2.0, Kd_trans=0.0, Kp_yaw=1.5, Kd_yaw=0.0, max_v=0.6, max_w=0.5)
state = G1BridgeState(pid=pid)
controller = G1Controller(
    network_interface=args.network_interface,
    dry_run=args.dry_run,
    call_start=args.call_start,
    loco_timeout=args.loco_timeout,
)
receiver = ZmqSyncReceiver(
    args.rgbd_sync_url,
    recv_timeout_ms=args.recv_timeout_ms,
    conflate=not args.no_conflate,
)
odom_receiver = Ros1OdomReceiver(args.odom_topic)

print(f"[run] stream RGB-D input from {args.rgbd_sync_url}")
print(f"[run] odom source: ROS1 topic {args.odom_topic}")
print(f"[run] server: {args.server_url}")
print("[run] control style: Go2-like planning_thread + control_thread + MPC/PID")
print("[run] target model: DualVLN-compatible /eval_dual server output")

planning_thread = threading.Thread(
    target=planning_thread_fn,
    args=(state, odom_receiver, receiver, args),
    daemon=True,
)
control_thread = threading.Thread(
    target=control_thread_fn,
    args=(state, controller, args),
    daemon=True,
)

planning_thread.start()
control_thread.start()

try:
    while True:
        time.sleep(1.0)
except KeyboardInterrupt:
    print("[run] interrupted by user")
finally:
    controller.stop()
    controller.close()
    receiver.close()
    odom_receiver.close()

if name == "main":
main()

[yuqiang-yang]

一般来说，走一到一半最终不走的原因有 1. 图像模糊了 2. 走的太慢了，没跟上轨迹，导致训推不一致了