Apollo 导航模式&相对地图

一、前期准备

官方使用说明：/apollo/modules/tools/navigator/README.md

1、录制数据，如20251215203344.record.00003 (有"/apollo/localization/pose"这个channel)

apollo 8.0中，解决运行py文件报错问题

export PYTHONPATH=$PYTHONPATH:/apollo:/apollo/bazel-bin

2、提取轨迹

cd /apollo/modules/tools/navigator
python3 record_extractor.py /apollo/20251215203344.record.00003

当前目录下生成path_20251215203344.record.00003.txt

-4551842.424522728,13494054.918855065
-4551842.4136810005,13494054.908842698
-4551842.402839273,13494054.898830328
-4551842.391997546,13494054.88881796
-4551842.370314093,13494054.868793227

可视化轨迹

python3 viewer_raw.py path_20251215203344.record.00003.txt

3、平滑轨迹

运行前修改modules/planning/reference_line/spiral_smoother_util.cc（添加换行符，否则有问题）

// before
    for (size_t i = 1; i + 1 < smoothed_points.size(); ++i) {
      const auto& point = smoothed_points[i];
      ofs << std::fixed << "{\"kappa\": " << point.kappa()
          << ", \"s\": " << point.s() << ", \"theta\": " << point.theta()
          << ", \"x\":" << point.x() << ", \"y\":" << point.y()
          << ", \"dkappa\":" << point.dkappa() << "}";
    }
// after
    for (size_t i = 1; i + 1 < smoothed_points.size(); ++i) {
      const auto& point = smoothed_points[i];
      ofs << std::fixed << "{\"kappa\": " << point.kappa()
          << ", \"s\": " << point.s() << ", \"theta\": " << point.theta()
          << ", \"x\":" << point.x() << ", \"y\":" << point.y()
          << ", \"dkappa\":" << point.dkappa() << "}\n";
    }

# 重新编译
./apollo.sh build

bash smooth.sh path_20251215203344.record.00003.txt 200

当前目录下生成path_20251215203344.record.00003.txt.smoothed

{"kappa": 0.000321474779, "s": 0.019912668453, "theta": -0.763918599152, "x":-4551842.373739121482, "y":13494054.870805589482, "dkappa":-0.000044209549}
{"kappa": 0.000320626641, "s": 0.039825336907, "theta": -0.763912206282, "x":-4551842.359359525144, "y":13494054.857030918822, "dkappa":-0.000040989451}
{"kappa": 0.000319841835, "s": 0.059738005360, "theta": -0.763905829667, "x":-4551842.344979841262, "y":13494054.843256337568, "dkappa":-0.000037848501}
{"kappa": 0.000319118791, "s": 0.079650673813, "theta": -0.763899468063, "x":-4551842.330600068904, "y":13494054.829481849447, "dkappa":-0.000034786049}
{"kappa": 0.000318455952, "s": 0.099563342266, "theta": -0.763893120254, "x":-4551842.316220209934, "y":13494054.815707452595, "dkappa":-0.000031801443}

可视化平滑过的轨迹

python3 viewer_smooth.py path_20251215203344.record.00003.txt path_20251215203344.record.00003.txt.smoothed

4、封装和发布

在Apollo v2.5下载/apollo/modules/tools/navigator/navigator.py （Apollo v2.5后的版本已经删除该文件），该文件为ros版本，修改成cyber版本如下

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import json
import time
from cyber.python.cyber_py3 import cyber
from modules.common_msgs.planning_msgs import navigation_pb2

def run_navigator(file_path):
    cyber.init()
    node = cyber.Node("navigator_node")
    navigation_pub = node.create_writer("/apollo/navigation", navigation_pb2.NavigationInfo)

    navigation_info = navigation_pb2.NavigationInfo()
    navigation_path = navigation_info.navigation_path.add()
    navigation_path.path_priority = 0
    navigation_path.path.name = "navigation"

    try:
        with open(file_path, 'r') as f:
            cnt = 0
            for line in f:
                cnt += 1
                if cnt < 3:
                    continue
                json_point = json.loads(line)
                point = navigation_path.path.path_point.add()
                point.x = json_point['x']
                point.y = json_point['y']
                point.s = json_point['s']
                point.theta = json_point['theta']
                point.kappa = json_point['kappa']
                point.dkappa = json_point['dkappa']
    except Exception as e:
        print(f"Error reading file: {e}")
        return

    print(navigation_info)

    rate = 1  # 1Hz
    while not cyber.is_shutdown():
        navigation_pub.write(navigation_info)
        print("Navigation info published.")
        time.sleep(1.0 / rate)
        # break 

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print("Usage: python3 navigator.py <data_file>")
        sys.exit(1)
        
    data_file = sys.argv[1]
    run_navigator(data_file)

python3 navigator.py path_20251215203344.record.00003.txt.smoothed

在cyber_monitor中可看到发布的信息

二、仿真测试

设置导航模式，在modules/common/data/global_flagfile.txt填入--use_navigation_mode=true，

1、启动dreamview（导航模式下，界面中mode会自动选择为Navigation，左上角小窗口会加载baidu地图，需要联网，不然点击任何按钮后将网页将卡死）

没有网时，禁止加载地图的办法：

修改modules/dreamview/frontend/src/components/Layouts/MainView.js

                {hmi.shouldDisplayNavigationMap
                    && (
                        <Navigation
                            onResize={() => dimension.toggleNavigationSize()}
                            hasRoutingControls={hmi.inNavigationMode}
                            {...dimension.navigation}
                        />
                    )}

改为：
                {hmi.shouldDisplayNavigationMap}

重新编译前端

1、用 nvm 安装并切换到 Node 18（或 20）：
curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.39.5/install.sh | bash
source ~/.nvm/nvm.sh
nvm install 18
nvm use 18
node -v # 应显示 v18.x.x
# 卸载 nvm uninstall 18.20.8
# 如果要回退到之前的版本，执行nvm deactivate

# 飞腾D2000上使用node18会有问题：
# nvm 默认会从官方下载预编译好的二进制包。Node.js 官方编译 v18 时使用了高版本的 GLIBC，而当前这个 Apollo 容器底层的 Linux 系统太老（基于 Ubuntu 18.04），没有 GLIBC_2.28，而有些依赖（favicons 和 image-webpack-loader）又必须用node18不然会报错。
# 解决：用空包“欺骗”系统
# 打开 /apollo/modules/dreamview/frontend/package.json，修改resolutions块，在末尾追加一条重定向规则 "**/sharp": "file:./sharp-mock"。
"resolutions": {
  "**/sharp": "file:./sharp-mock"
}
# 1. 强制清理之前卡死或有残留的 node_modules 目录
rm -rf node_modules sharp-mock
# 2. 创建本地的本地伪装目录
mkdir -p sharp-mock
# 3. 写入声明文件，版本号写 99.9.9 保证可以匹配任何第三方包的依赖诉求
echo '{"name": "sharp", "version": "99.9.9", "main": "index.js"}' > sharp-mock/package.json
# 4. 写入一个标准的 JavaScript 动态代理，让任何调用 sharp 的地方都不会产生未定义崩溃
echo 'module.exports = new Proxy({}, { get: () => () => ({}) });' > sharp-mock/index.js

rm -rf node_modules yarn.lock
yarn cache clean
# 加上--ignore-engines，否则报需要node>=18的错误
yarn install --ignore-optional --ignore-engines
yarn add @babel/plugin-transform-runtime --dev --ignore-engines #在/apollo/modules/dreamview/frontend 目录下运行

2、清理缓存与旧依赖：
cd /apollo/modules/dreamview/frontend
rm -rf dist
rm -rf node_modules yarn.lock
yarn cache clean
3、重新安装依赖并安装 webpack-cli：
yarn install --ignore-optional
yarn add -D webpack-cli@4
yarn add --dev @babel/plugin-proposal-optional-chaining
4、重建前端：
cd /apollo
./apollo.sh build_fe

启动dreamview

bash scripts/bootstrap.sh
# bash scripts/bootstrap.sh restart
# bash scripts/bootstrap.sh stop
# 打开 http://localhost:8888 就可以查看dreamview界面

2、启动relativemap模块

modules/map/relative_map/conf/relative_map_config.pb.txt里可设置道路宽度、搜索参考线范围

mainboard -d /apollo/modules/map/relative_map/dag/relative_map.dag
# 报错[mainboard]Invalid localization input.，播包后有定位就好了

如果报错
E0127 15:38:28.850536 20508 class_loader_manager.h:70] [mainboard]Invalid class name: apollo::relative_map::RelativeMapComponent
E0127 15:38:28.850564 20508 module_controller.cc:67] [mainboard]Failed to load module: /apollo/modules/map/relative_map/dag/relative_map.dag
E0127 15:38:28.850790 20508 mainboard.cc:39] [mainboard]module start error.

解决：在BUILD中加入alwayslink = True,确保注册宏不被编译器剔除

cc_library(
    name = "relative_map_component_lib",
    srcs = ["relative_map_component.cc"],
    hdrs = ["relative_map_component.h"],
    alwayslink = True,
    copts = MAP_COPTS,
    deps = [
        ":relative_map_lib",
        "//cyber",
        "//modules/common/adapters:adapter_gflags",
        "@com_github_gflags_gflags//:gflags",
    ],
)

3、启动定位（这里直接播放数据包）

cyber_recorder play -f 20251215203344.record.00003 -c /apollo/localization/pose

4、发布导航轨迹

我把上面平滑后的轨迹数据分成多段，分别放在test.json, test2.json ...里面

先发布第一段导航轨迹

python3 modules/tools/navigator/navigator.py modules/tools/navigator/test.json

dreamview界面

播包等车走到第一段轨迹尽头，再发布第二段轨迹导航轨迹

python3 modules/tools/navigator/navigator.py modules/tools/navigator/test2.json

dreamview界面

三、实际使用

思路：c++写一个组件或节点，接收经纬度格式的轨迹段，将经纬度按照local_utm_zone_id的值转化为UTM坐标，封装成NavigationInfo，并发布到"/apollo/navigation"

假设需要跟踪的路径的经纬度在path.txt中

116.2685391,39.9256317
116.2685391,39.9256318
116.2685391,39.9256319
116.2685391,39.925632
116.2685391,39.925632199999995
116.26854069999999,39.9257006
116.26854259999999,39.9257645
116.2685442,39.9258233
116.2685448,39.9258451
116.2685453,39.9258637
116.2685417,39.9258952

任务的内容写在task.json文件里面

{
    "start": 1,
    "gear_location": 0,
    "steering_target": 0,
    "speed": 0,
    "turn_signal": 0,
    "reset_model": false,
    "enable_send_path": true,
    "task_path": {
        "path_id": 1,
        "path_file": "/apollo/modules/navigation/path.txt"
    }
}

对应的协议文件task.proto

syntax = "proto2";

package apollo.navigation;

message LonLat {
    optional double lon = 1;
    optional double lat = 2;
}

message TaskPath {
    optional int32 path_id = 1;
    repeated LonLat lon_lat = 2;
    optional string path_file = 3;
}

message Task {
    optional int32 start = 1;
    optional int32 gear_location = 3;
    optional double steering_target = 4;
    optional double speed = 5;
    optional int32 turn_signal = 6;
    optional bool reset_model = 7;

    optional bool enable_send_path = 8;    
    optional TaskPath task_path = 9;
}

注意：apollo中，如果使用的 cyber_monitor 版本与当前的 Protobuf 消息定义不兼容，会导致cyber_monitor挂掉Segmentation fault (core dumped)，这里Protobuf消息定义如果使用proto3会挂掉，proto2就不会。

先写个task.py文件来模拟上游下发的任务（包括要跟踪的路径、控制命令等）

import sys
import json
import time
from google.protobuf import json_format
sys.path.append("/apollo") # 如果找不到cyber
from cyber.python.cyber_py3 import cyber
sys.path.append("/apollo/bazel-bin") # 如果找不到navigation_pb2
from modules.common_msgs.planning_msgs import navigation_pb2
from modules.navigation import task_pb2

if __name__ == '__main__':
    task_file = "/apollo/modules/navigation/task.json"
    with open(task_file, 'r') as f:
        task_json = f.read()
        task_proto = task_pb2.Task()
        json_format.Parse(task_json, task_proto, ignore_unknown_fields=True)

    lonlat_file = task_proto.task_path.path_file
    with open(lonlat_file, 'r') as f:
        for line in f:
            lonlat_str = line.split(",")
            lon = float(lonlat_str[0])
            lat = float(lonlat_str[1])

            lonlat = task_proto.task_path.lon_lat.add()
            lonlat.lon = lon
            lonlat.lat = lat

    # print(task_proto)

    cyber.init()
    node = cyber.Node("task_node")
    task_pub = node.create_writer("/apollo/task", task_pb2.Task)

    rate = 1  # 1Hz
    while not cyber.is_shutdown():
         task_pub.write(task_proto)
         print("task published.")
         time.sleep(1.0 / rate)
         # break 
    cyber.shutdown()

/apollo/task里的消息就可以模拟上游发过来的任务

仿照apollo dag的框架添加一个新模块，如叫navigation

navigation_component.h

// Copyright 2026 The alan Authors. All Rights Reserved.

#pragma once

#include <atomic>
#include <thread>
#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
#include <proj_api.h>

#include "cyber/class_loader/class_loader.h"
#include "cyber/component/component.h"
#include "cyber/cyber.h"
#include "cyber/message/raw_message.h"

#include "modules/common_msgs/planning_msgs/navigation.pb.h"
#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
#include "modules/navigation/task.pb.h"

namespace apollo {
namespace navigation {

class NavigationComponent final
    : public cyber::Component<navigation::Task> {
 public:
  NavigationComponent();
  ~NavigationComponent();

  bool Init() override;
  bool Proc(const std::shared_ptr<navigation::Task> &task_msg) override;

 private:
  void PublishControlCmd();
  void PublishNavigationInfo(const relative_map::NavigationInfo &navigation_info);

 private:
  std::string navigation_info_topic_ = "/apollo/navigation";
  std::string control_cmd_topic_ = "/apollo/control";
  double control_cmd_pub_rate_{15}; // 实测12hz以上才能控车？
  std::shared_ptr<cyber::Writer<relative_map::NavigationInfo>>
      navigation_info_writer_ = nullptr;
  std::shared_ptr<cyber::Writer<control::ControlCommand>>
      control_cmd_writer_ = nullptr;
  projPJ pj_latlon_;
  projPJ pj_utm_;
  control::ControlCommand control_cmd_;
  relative_map::NavigationInfo navigation_info_;
  std::unique_ptr<std::thread> control_cmd_pub_thread_;
  std::atomic<bool> is_running_{true};
};

CYBER_REGISTER_COMPONENT(NavigationComponent);

}  // namespace navigation
}  // namespace apollo

navigation_component.cc

// Copyright 2026 The alan Authors. All Rights Reserved.

#include "absl/strings/str_cat.h"
#include "modules/common/util/message_util.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/navigation/navigation_component.h"

namespace apollo {
namespace navigation {

constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
// proj4_text: "+proj=utm +zone=10 +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no_defs"

using apollo::canbus::Chassis;
using apollo::common::VehicleSignal;

NavigationComponent::NavigationComponent() {}

NavigationComponent::~NavigationComponent() {
  is_running_ = false;
  control_cmd_pub_thread_->join();
  std::cout << "exit control_cmd_pub_thread !" << std::endl;
}

bool NavigationComponent::Init() {
  navigation_info_writer_ =
      node_->CreateWriter<relative_map::NavigationInfo>(navigation_info_topic_);
  control_cmd_writer_ =
      node_->CreateWriter<control::ControlCommand>(control_cmd_topic_);
  ACHECK(navigation_info_writer_);
  ACHECK(control_cmd_writer_);

  std::string latlon_src =
      "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
  std::string utm_dst =
      absl::StrCat("+proj=utm +zone=", FLAGS_local_utm_zone_id, " +ellps=GRS80 +units=m +no_defs");
  if (!(pj_latlon_ = pj_init_plus(latlon_src.c_str()))) {
    return false;
  }
  if (!(pj_utm_ = pj_init_plus(utm_dst.c_str()))) {
    return false;
  }
  control_cmd_pub_thread_.reset(
        new std::thread([this] { PublishControlCmd(); }));
  return true;
}

bool NavigationComponent::Proc(
    const std::shared_ptr<navigation::Task>& task_msg) {
    // ########### control cmd ##########
    // pad_msg,start
    int32_t int_action = task_msg->start();
    control::DrivingAction action =
        control::DrivingAction::RESET;
    switch (int_action) {
      case 0:
        action = control::DrivingAction::RESET;
        AINFO << "SET Action RESET";
        break;
      case 1:
        action = control::DrivingAction::START;
        AINFO << "SET Action START";
        break;
      default:
        AINFO << "unknown action: " << int_action << " use default RESET";
        break;
    }
    control::PadMessage pad_msg;
    pad_msg.set_action(action);
    control_cmd_.mutable_pad_msg()->CopyFrom(pad_msg);
    // control_cmd_.clear_pad_msg();

    // gear_location
    int gear_location = task_msg->gear_location();
    Chassis::GearPosition gear = Chassis::GEAR_INVALID;
    switch (gear_location) {
      case 0:
        gear = Chassis::GEAR_NEUTRAL;
        break;
      case 1:
        gear = Chassis::GEAR_DRIVE;
        break;
      case 2:
        gear =  Chassis::GEAR_REVERSE;
        break;
      case 3:
        gear =  Chassis::GEAR_PARKING;
        break;
      case 4:
        gear =  Chassis::GEAR_LOW;
        break;
      case 5:
        gear =  Chassis::GEAR_INVALID;
        break;
      case 6:
        gear =  Chassis::GEAR_NONE;
        break;
      default:
        gear =  Chassis::GEAR_INVALID;
        break;
    }
    control_cmd_.set_gear_location(gear);

    // steering_target
    int steering_target = task_msg->steering_target();
    if (steering_target > 100) {
      steering_target = 100;
    } else if (steering_target < -100) {
      steering_target = -100;
    }
    control_cmd_.set_steering_target(steering_target);

    // set_speed
    double speed = task_msg->speed();
    if (speed > 3) {
      speed = 3;
    } else if (speed < -3) {
      speed = -3;
    }
    control_cmd_.set_speed(speed);
  
    // turn_signal
    int turn_signal = task_msg->turn_signal();
    VehicleSignal::TurnSignal signal = VehicleSignal::TURN_NONE;
    switch (turn_signal) {
      case 0:
        signal = VehicleSignal::TURN_NONE;
        break;
      case 1:
        signal = VehicleSignal::TURN_LEFT;
        break;
      case 2:
        signal = VehicleSignal::TURN_RIGHT;
        break;
      default:
        signal = VehicleSignal::TURN_NONE;
        break;
    }
    control_cmd_.mutable_signal()->set_turn_signal(signal);

    // ######## navigation path ############
    if (!task_msg->enable_send_path()) {
      return true;
    }
    navigation_info_.Clear();
    relative_map::NavigationPath* navigation_path = navigation_info_.add_navigation_path();
    navigation_path->set_path_priority(0);
    common::Path* path = navigation_path->mutable_path();
    path->set_name(std::to_string(task_msg->task_path().path_id()));

    const auto task_path = task_msg->task_path();
    for (const auto& ll : task_path.lon_lat()) {
        double longitude = ll.lon();
        double latitude = ll.lat();
        longitude *= DEG_TO_RAD_LOCAL;
        latitude *= DEG_TO_RAD_LOCAL;
        pj_transform(pj_latlon_, pj_utm_, 1, 1, &longitude, &latitude, nullptr);
        common::PathPoint path_point;
        path_point.set_x(longitude);
        path_point.set_y(latitude);
        *(path->add_path_point()) = path_point;
    }
    PublishNavigationInfo(navigation_info_);
    return true;
}

void NavigationComponent::PublishControlCmd() {
  cyber::Rate rate(control_cmd_pub_rate_);
  while (is_running_) {
    // header
    common::util::FillHeader("control", &control_cmd_);
    // std::cout << "cmd:\n" << control_cmd_.ShortDebugString() << std::endl;
    control_cmd_writer_->Write(control_cmd_);
    // std::cout << "publish control_cmd !" << std::endl;
    rate.Sleep();
  }
}

void NavigationComponent::PublishNavigationInfo(
    const relative_map::NavigationInfo &navigation_info_) {
  navigation_info_writer_->Write(navigation_info_);
  std::cout << "publish navigation_info !" << std::endl;
}

}  // namespace navigation
}  // namespace apollo

BUILD

load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test")
load("//tools/install:install.bzl", "install")
load("//tools:cpplint.bzl", "cpplint")
load("@rules_proto//proto:defs.bzl", "proto_library")
load("@rules_cc//cc:defs.bzl", "cc_proto_library")
load("//tools:python_rules.bzl", "py_proto_library")

package(default_visibility = ["//visibility:public"])

proto_library(
    name = "task_proto",
    srcs = ["task.proto"],
    deps = [
    ],
)

cc_proto_library(
    name = "task_cc_proto",
    deps = [
        ":task_proto",
    ],
)

py_proto_library(
    name = "task_py_pb2",
    deps = [
        ":task_proto",
    ],
)

cc_library(
    name = "navigation_component_lib",
    srcs = [
        "navigation_component.cc",
    ],
    hdrs = [
        "navigation_component.h",
    ],
    copts = ["-DMODULE_NAME=\\\"navigation\\\""],
    alwayslink = True,
    deps = [
        ":task_cc_proto",
        "@com_google_absl//:absl",
        "@proj",
        "//cyber",
        "//modules/common/util:util_tool",
        "//modules/common/configs:config_gflags",
        "//modules/common_msgs/planning_msgs:navigation_cc_proto",
        "//modules/common_msgs/control_msgs:control_cmd_cc_proto",
    ],
)

cc_binary(
    name = "libnavigation_component.so",
    linkshared = True,
    linkstatic = True,
    deps = [":navigation_component_lib"],
)

cpplint()

navigation.dag

module_config {
    module_library : "/apollo/bazel-bin/modules/navigation/libnavigation_component.so"

    components {
      class_name : "NavigationComponent"
      config {
        name : "navigation"
        # config_file_path : "/apollo/modules/navigation/conf/navigation.pb.txt"
        readers: [
          {
            channel: "/apollo/task"
            qos_profile: {
              depth : 1
            }
            pending_queue_size: 1
          }
        ]
      }
    }
}

测试：

在dreamview里启动canbus, relativemap, navigation三个模块，并播放一小会数据包

cyber_recorder play -f 20251215203344.record.00003 -c /apollo/localization/pose 

发布任务

python3 task.py

可以看到车辆进入自动驾驶控制模式，并且dreamview界面显示了要跟踪的轨迹

（注意：电量低于50%无法进入自动驾驶控制模式）