Apollo項目代碼遷移到Cyber RT框架（Apollo 3.5以上版本）的方法

嚴正聲明：本文系作者davidhopper原創，未經許可，不得轉載。

Apollo 3.5徹底摒棄ROS，改用自研的Cyber RT作爲底層通訊與調度平臺，實時性與靈活性更爲突出。關於Apollo 3.5的構建方法，可參見我的一篇博客。關於Apollo 3.5各功能模塊的啓動過程解析，可參見我的另一篇博客。
本文闡述Apollo項目代碼遷移到Cyber RT框架（Apollo 3.5以上版本）的方法。本文在Apollo官方文檔：How to create and run a new component in Apollo Cyber RT及How to use Cyber commands的基礎上撰寫，但更爲翔實具體。

一、功能模塊代碼的遷移

Apollo Cyber RT框架基於組件的概念構建、加載各功能模塊。Localization、 Perception、Prediction、Planning、Control等功能模塊均作爲Apollo Cyber RT框架的一個組件而存在，基於Cyber RT提供的調度程序mainboard加載運行。
基於Apollo Cyber RT框架創建和發佈新的功能模塊組件，需執行以下五個基本步驟：

• 設置組件文件結構
• 實現組件類
• 提供構建文件
• 提供配置文件
• 啓動組件

下面以Planning模塊爲例進行闡述。

1.1 設置組件文件結構

基於路徑${APOLLO_HOME}/modules/planning（${APOLLO_HOME}表示Apollo項目的根目錄，以我的機器爲例，Docker外部爲/home/davidhopper/code/apollo，Docker內部自不必說，全部爲/apollo。爲描述簡單起見，下文全部以Docker內部的路徑/apollo爲準）設置如下組件文件結構：

• 頭文件: planning_component.h；
• 實現文件: planning_component.cc；
• 構建文件: BUILD；
• DAG配置文件: dag/planning.dag；
• Launch配置文件: launch/planning.launch。

1.2 實現組件類

執行以下步驟以實現組件類：

• 基於模板類Component派生出規劃模塊的組件類PlanningComponent；
• 在派生類PlanningComponent中覆蓋要虛函數Init() and Proc()函數；
• 使用宏CYBER_REGISTER_COMPONENT(PlanningComponent)註冊組件類PlanningComponent，以便Cyber RT能正確創建並加載該類對象（關於該宏的具體含義，參見我的博客Apollo 3.5各功能模塊的啓動過程解析）。

1.2.1 組件類PlanningComponent的聲明

namespace apollo {
namespace planning {

class PlanningComponent final
    : public cyber::Component<prediction::PredictionObstacles, canbus::Chassis,
                              localization::LocalizationEstimate> {
 public:
  PlanningComponent() = default;

  ~PlanningComponent() = default;

 public:
  bool Init() override;

  bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&
                prediction_obstacles,
            const std::shared_ptr<canbus::Chassis>& chassis,
            const std::shared_ptr<localization::LocalizationEstimate>&
                localization_estimate) override;

 private:
  void CheckRerouting();
  bool CheckInput();

  std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>>
      traffic_light_reader_;
  std::shared_ptr<cyber::Reader<routing::RoutingResponse>> routing_reader_;
  std::shared_ptr<cyber::Reader<planning::PadMessage>> pad_message_reader_;
  std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;

  std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;
  std::shared_ptr<cyber::Writer<routing::RoutingRequest>> rerouting_writer_;

  std::mutex mutex_;
  perception::TrafficLightDetection traffic_light_;
  routing::RoutingResponse routing_;
  PadMessage pad_message_;
  relative_map::MapMsg relative_map_;

  LocalView local_view_;

  std::unique_ptr<PlanningBase> planning_base_;

  PlanningConfig config_;
};

CYBER_REGISTER_COMPONENT(PlanningComponent)

}  // namespace planning
}  // namespace apollo

注意到基類Component的定義爲：

template <typename M0 = NullType, typename M1 = NullType,
          typename M2 = NullType, typename M3 = NullType>
class Component : public ComponentBase {
 // ...
};

可見，Component類最多接受4個模板參數，每個模板參數均表示一種輸入的消息類型，這些消息在Proc函數中被週期性地接收並處理；而PlanningComponent繼承的是該模板類接受3個參數的一個特化版本：

template <typename M0, typename M1, typename M2>
class Component<M0, M1, M2, NullType> : public ComponentBase {
 // ...
};

即PlanningComponent繼承自cyber::Component<prediction::PredictionObstacles, canbus::Chassis, localization::LocalizationEstimate>，3個消息參數分別爲：prediction::PredictionObstacles、canbus::Chassis、localization::LocalizationEstimate，這些消息在Proc函數中被週期性地接收並處理。

1.2.2 組件類PlanningComponent的實現

PlanningComponent的實現主要包括兩個覆蓋的虛函數Init() and Proc()函數：

bool PlanningComponent::Init() {
  if (FLAGS_open_space_planner_switchable) {
    planning_base_ = std::unique_ptr<PlanningBase>(new OpenSpacePlanning());
  } else {
    planning_base_ = std::unique_ptr<PlanningBase>(new StdPlanning());
  }
  CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                               &config_))
      << "failed to load planning config file " << FLAGS_planning_config_file;
  planning_base_->Init(config_);

  if (FLAGS_use_sim_time) {
    Clock::SetMode(Clock::MOCK);
  }
  routing_reader_ = node_->CreateReader<RoutingResponse>(
      FLAGS_routing_response_topic,
      [this](const std::shared_ptr<RoutingResponse>& routing) {
        AINFO << "Received routing data: run routing callback."
              << routing->header().DebugString();
        std::lock_guard<std::mutex> lock(mutex_);
        routing_.CopyFrom(*routing);
      });
  traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
      FLAGS_traffic_light_detection_topic,
      [this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
        ADEBUG << "Received traffic light data: run traffic light callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        traffic_light_.CopyFrom(*traffic_light);
      });

  if (FLAGS_use_navigation_mode) {
    pad_message_reader_ = node_->CreateReader<PadMessage>(
        FLAGS_planning_pad_topic,
        [this](const std::shared_ptr<PadMessage>& pad_message) {
          ADEBUG << "Received pad data: run pad callback.";
          std::lock_guard<std::mutex> lock(mutex_);
          pad_message_.CopyFrom(*pad_message);
        });
    relative_map_reader_ = node_->CreateReader<MapMsg>(
        FLAGS_relative_map_topic,
        [this](const std::shared_ptr<MapMsg>& map_message) {
          ADEBUG << "Received relative map data: run relative map callback.";
          std::lock_guard<std::mutex> lock(mutex_);
          relative_map_.CopyFrom(*map_message);
        });
  }
  planning_writer_ =
      node_->CreateWriter<ADCTrajectory>(FLAGS_planning_trajectory_topic);

  rerouting_writer_ =
      node_->CreateWriter<RoutingRequest>(FLAGS_routing_request_topic);

  return true;
}

其中Init()函數用於創建實際規劃類對象，創建除prediction::PredictionObstacles、canbus::Chassis、localization::LocalizationEstimate三類消息以外的其他消息處理回調函數，創建Planning模塊的輸出器：軌跡輸出器planning_writer_和重新生成路由輸出器rerouting_writer_。注意目前（2019年1月7日）版本並未創建導航模式規劃器NaviPlanning。

bool PlanningComponent::Proc(
    const std::shared_ptr<prediction::PredictionObstacles>&
        prediction_obstacles,
    const std::shared_ptr<canbus::Chassis>& chassis,
    const std::shared_ptr<localization::LocalizationEstimate>&
        localization_estimate) {
  CHECK(prediction_obstacles != nullptr);

  if (FLAGS_use_sim_time) {
    Clock::SetNowInSeconds(localization_estimate->header().timestamp_sec());
  }
  // check and process possible rerouting request
  CheckRerouting();

  // process fused input data
  local_view_.prediction_obstacles = prediction_obstacles;
  local_view_.chassis = chassis;
  local_view_.localization_estimate = localization_estimate;
  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!local_view_.routing ||
        hdmap::PncMap::IsNewRouting(*local_view_.routing, routing_)) {
      local_view_.routing =
          std::make_shared<routing::RoutingResponse>(routing_);
      local_view_.is_new_routing = true;
    } else {
      local_view_.is_new_routing = false;
    }
  }
  {
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.traffic_light =
        std::make_shared<TrafficLightDetection>(traffic_light_);
  }

  if (!CheckInput()) {
    AERROR << "Input check failed";
    return false;
  }

  ADCTrajectory adc_trajectory_pb;
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  auto start_time = adc_trajectory_pb.header().timestamp_sec();
  common::util::FillHeader(node_->Name(), &adc_trajectory_pb);

  // modify trajecotry relative time due to the timestamp change in header
  const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
  for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
    p.set_relative_time(p.relative_time() + dt);
  }
  planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
  return true;
}

而Proc()函數週期性地接收prediction::PredictionObstacles、canbus::Chassis、localization::LocalizationEstimate三類消息，調用planning_base_->RunOnce()函數執行實際的路徑與速度規劃，並將規劃結果adc_trajectory_pb藉助函數planning_writer_->Write()將生成的規劃軌跡輸出給控制模塊執行。

1.3 提供構建文件/apollo/modules/planning/BUILD

下面列出/apollo/modules/planning/BUILD文件中與Cyber RT相關的內容，可見基於planning_component_lib庫最終生成了一個共享庫文件libplanning_component.so，而該共享庫通過Cyber RT調度程序mainboard動態加載運行：

load("//tools:cpplint.bzl", "cpplint")

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

cc_library(
    name = "planning_component_lib",
    srcs = [
        "planning_component.cc",
    ],
    hdrs = [
        "planning_component.h",
    ],
    copts = [
        "-DMODULE_NAME=\\\"planning\\\"",
    ],
    deps = [
        ":planning_lib",
        "//cyber",
        "//modules/common/adapters:adapter_gflags",
        "//modules/common/util:message_util",
        "//modules/localization/proto:localization_proto",
        "//modules/map/relative_map/proto:navigation_proto",
        "//modules/perception/proto:perception_proto",
        "//modules/planning/proto:planning_proto",
        "//modules/prediction/proto:prediction_proto",
    ],
)

cc_binary(
    name = "libplanning_component.so",
    linkshared = True,
    linkstatic = False,
    deps = [":planning_component_lib"],
)

# ...

1.4 提供DAG配置文件: /apollo/dag/planning.dag

DAG配置文件是Cyber RT調度程序mainboard動態加載Planning模塊的最終配置文件，加載命令一般爲：

/apollo/bazel-bin/cyber/mainboard -d /apollo/modules/planning/dag/planning.dag

標準模式的DAG配置文件如下：

# Define all coms in DAG streaming.
module_config {
  # 共享庫文件路徑
  module_library : "/apollo/bazel-bin/modules/planning/libplanning_component.so"
  components {
    # 組件類名稱，一定不能寫錯，否則mainboard無法動態創建PlanningComponent組件對象
    class_name : "PlanningComponent"
    config {
      # 模塊名
      name: "planning"
      # GFlag配置文件路徑，注意路徑一定寫成絕對路徑，否則可能無法找到配置文件，導致模塊加載失敗
      flag_file_path:  "/apollo/modules/planning/conf/planning.conf"
      # PlanningComponent組件Proc()函數中使用的三個消息接收器
      readers: [
        {
          channel: "/apollo/prediction"
        },
        {
          channel: "/apollo/canbus/chassis"
          qos_profile: {
              depth : 15
          }
          pending_queue_size: 50
        },
        {
          channel: "/apollo/localization/pose"
          qos_profile: {
              depth : 15
          }
          pending_queue_size: 50
        }
      ]
    }
  }
}

1.5 提供Launch配置文件: /apollo/launch/planning.launch

Launch配置文件是Cyber RT提供的一個Python工具程序cyber_launch加載Planning模塊所需的配置文件，啓動命令如下所示（最終仍歸結於mainboard加載）：

cyber_launch start /apollo/launch/planning.launch

標準模式的Launch配置文件如下：

<cyber>
    <module>
        <name>planning</name>
        <dag_conf>/apollo/modules/planning/dag/planning.dag</dag_conf>
        <process_name>planning</process_name>
    </module>
</cyber>

1.6 如何接收消息？

基於Cyber RT接收消息分兩種情形，第一種是1.2.1節描述的在虛函數PlanningComponent::Proc()中處理指定的消息類型，這類消息是週期性觸發，但最多隻能接收4種（因爲cyber::Component的模板參數最多隻有4個），一般用於模塊主要輸入消息的接收。第二種是直接創建消息接收器，一般用於接收非週期性消息或模塊的次要輸入消息，示例代碼如下，注意消息處理回調函數均以Lambda表達式的方式展現：

 routing_reader_ = node_->CreateReader<RoutingResponse>(
      FLAGS_routing_response_topic,
      [this](const std::shared_ptr<RoutingResponse>& routing) {
        AINFO << "Received routing data: run routing callback."
              << routing->header().DebugString();
        std::lock_guard<std::mutex> lock(mutex_);
        routing_.CopyFrom(*routing);
      });
  traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
      FLAGS_traffic_light_detection_topic,
      [this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
        ADEBUG << "Received traffic light data: run traffic light callback.";
        std::lock_guard<std::mutex> lock(mutex_);
        traffic_light_.CopyFrom(*traffic_light);
      });

1.7 如何發佈消息？

基於Cyber RT發佈消息非常直觀，首先創建發佈器對象，然後填充消息，最後發佈消息，示例代碼如下：

  // 1.創建發佈器
  planning_writer_ =
      node_->CreateWriter<ADCTrajectory>(FLAGS_planning_trajectory_topic);
  
  // 2.填充消息
  ADCTrajectory adc_trajectory_pb;
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  auto start_time = adc_trajectory_pb.header().timestamp_sec();
  common::util::FillHeader(node_->Name(), &adc_trajectory_pb);

  // modify trajecotry relative time due to the timestamp change in header
  const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
  for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
    p.set_relative_time(p.relative_time() + dt);
  }
  
  // 3.發佈消息
  planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));

1.8 如何在main()函數中單獨使用Cyber RT？

Cyber RT可以在main()函數中單獨使用，示例代碼如下，更多示例可查看Cyber examples：

int main(int argc, char** argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  // Init the cyber framework
  apollo::cyber::Init(argv[0]);
  FLAGS_alsologtostderr = true;

  NavigationInfo navigation_info;
  // ...

  std::shared_ptr<apollo::cyber::Node> node(
      apollo::cyber::CreateNode("navigation_info"));
  auto writer = node->CreateWriter<apollo::relative_map::NavigationInfo>(
      FLAGS_navigation_topic);

  // In theory, the message only needs to be sent once. Considering the problems
  // such as the network delay, We send it several times to ensure that the data
  // is sent successfully.
  Rate rate(1.0);
  constexpr int kTransNum = 3;
  int trans_num = 0;
  while (apollo::cyber::OK()) {
    if (trans_num > kTransNum) {
      break;
    }
    apollo::common::util::FillHeader(node->Name(), &navigation_info);
    writer->Write(navigation_info);
    ADEBUG << "Sending navigation info:" << navigation_info.DebugString();
    rate.Sleep();
    ++trans_num;
  }

  return 0;
}

二、輔助工具的遷移

2.1 常用工具對比

Apollo 3.0以下版本提供了許多基於ROS的調試工具，Apollo 3.5的Cyber RT框架同樣提供了類似功能，下面給出常用工具的對比表：

ROS	Cyber	備註
rosbag	cyber_recorder	處理數據文件
rostopic	cyber_channel	查看某個topic的信息
scripts/diagnostics.sh	cyber_monitor	查看診斷消息
offline_lidar_visualizer_tool	cyber_visualizer	激光點雲及攝像頭可視化工具，需要安裝NVIDIA顯卡驅動及CUDA

2.2 常用命令遷移

ROS	Cyber	備註
rosbag play example.bag	cyber_recorder play -f example.record	播放一個包
rosbag info example.bag	cyber_recorder info example.record	查看一個包的信息
rosbag record /apollo/canbus/chassis \ /apollo/canbus/chassis_detail	cyber_recorder record -c /apollo/canbus/chassis \ /apollo/canbus/chassis_detail	錄製多個topic
rosbag filter input.bag output.bag ‘topic != “/apollo/planning”’	cyber_recorder split -f input_file.record -o ouput_file.record -k “/apollo/planning”	濾除一個topic
rosbag filter csc.bag csc_no_plannig_and_relativemap.bag ‘topic != “/apollo/planning” and “/apollo/relative_map”’	cyber_recorder split -f input_file.record -o ouput_file.record -k “/apollo/planning” -k “/apollo/relative_map”’	濾除多個topic
rostopic list	cyber_channel list	列出所有活動的topic
rostopic info /apollo/planning	cyber_channel info /apollo/planning	查看 /apollo/planning topic的概要信息
rostopic echo /apollo/planning	cyber_channel echo /apollo/planning	查看 /apollo/planning topic的內容
rostopic hz /apollo/planning	cyber_channel hz /apollo/planning	查看 /apollo/planning topic的發送頻率
rostopic bw /apollo/planning	cyber_channel bw /apollo/planning	查看 /apollo/planning topic的帶寬
rostopic type /apollo/planning	cyber_channel type /apollo/planning	查看 /apollo/planning topic的數據類型

示例：

cyber_recorder record -c /apollo/localization/pose /apollo/canbus/chassis /apollo/perception/obstacles /apollo/prediction /apollo/planning /apollo/control

三、GDB調試功能的遷移

3.1 調試啓動命令

下面給出Apollo 3.0以下版本及Apollo 3.5以上版本的GDB調試啓動命令：

### 方法1：直接啓動模塊調試
# Apollo 3.0以下版本GDB調試啓動命令
gdb -q --args bazel-bin/modules/planning/planning --flagfile=/apollo/modules/planning/conf/planning.conf

# Apollo 3.5以上版本GDB調試啓動命令
gdb -q --args bazel-bin/cyber/mainboard -d /apollo/modules/planning/dag/planning.dag

### 方法2：通過Dreamview啓動相關模塊，附加調試相關進程
# Apollo 3.0以下版本GDB調試啓動命令
# 在Dreamview中啓動Planning模塊，然後使用ps aux | grep planning命令查找
# planning進程ID（PID），假設爲35872，則使用attach模式附加到當前planning進程調試
sudo gdb -q bazel-bin/modules/planning/planning -p 35872

# # Apollo 3.5以上版本GDB調試啓動命令
# 在Dreamview中啓動Planning模塊，然後使用ps aux | grep mainboard命令查找
# 帶有“mainboard -d /apollo/modules/planning/dag/planning.dag”描述字符的mainboard進程ID（PID），
# 假設爲35872，則使用attach模式附加到mainboard進程調試
sudo gdb -q bazel-bin/cyber/mainboard -p 35872

值得指出的是，因爲Apollo 3.5以上版本通過動態創建的方式啓動Planning模塊，因此在使用GDB設置斷點時，按下TAB鍵不會有提示，可以藉助VSCode提供的Copy Relative Path功能撰寫正確的源代碼文件路徑，如下圖所示：

3.2 調試示例

3.3 特殊情況處理

3.3.1 人工發送prediction::PredictionObstacles消息

爲提高消息處理的實時性和靈活性，Apollo 3.5的Planning模塊不再基於定時器觸發更新，而是基於三個輸入消息的改變而動態更新，這三個輸入消息分別爲：prediction::PredictionObstacles、canbus::Chassis、localization::LocalizationEstimate。也就是說，只有上述三個消息同時存在時，Planning模塊的消息處理函數PlanningComponent::Proc()纔會被調用，而具體的某一類規劃算法（例如OnLanePlanning）纔會真正工作。

若某條消息因爲特殊原因不能及時發送，解決辦法就是人工生成假消息。例如，若不能收到prediction::PredictionObstacles消息，則可在在Docker內部通過如下命令生成假prediction::PredictionObstacles消息：

cyber_launch start /apollo/modules/tools/prediction/fake_prediction/fake_prediction.launch

該假消息的具體生成代碼見/apollo/modules/tools/prediction/fake_prediction，其他假消息的生成可參照該示例撰寫。

3.3.2 人工發送perception::TrafficLightDetection消息

調試規劃算法時，需要動態改變紅綠燈的信號狀態，可以通過如下命令人工發送perception::TrafficLightDetection消息來實現：

cyber_launch start /apollo/modules/tools/manual_traffic_light/manual_traffic_light.launch

程序啓動後，按c鍵和回車鍵，可以動態切換紅綠燈狀態。

四、ROS bag數據包的遷移

如果之前使用ROS錄製了很多bag數據包，當然不能輕易浪費這些資源。所幸Cyber RT充分考慮到該問題，已爲我們提供了轉換工具rosbag_to_record，下面將一個Apollo 2.5 demo bag轉換爲Cyber RT支持的record格式數據包：

rosbag_to_record demo_2.5.bag demo.record

關於該轉換工具的更多描述，請參見Apollo幫助文檔。

五、ROS讀寫.bag文件功能的遷移

如下所示，ROS提供了直接從.bag文件讀取、分析數據的功能：

rosbag::Bag bag;
try {
  bag.open(bag_filename);  // BagMode is Read by default
} catch (const rosbag::BagException& e) {
  AERROR << "Can't open the input bag file: " << bag_filename;
  AERROR << "The reason is: " << e.what();
  return false;
}

std::vector<std::string> topics = {"/apollo/navi_generator/collector"};
rosbag::View view(bag, rosbag::TopicQuery(topics));
for (const auto& message : view) {
  auto msg = message.instantiate<TrajectoryCollectorMsg>();
  if (msg != nullptr) {
    *min_speed_limit = msg->min_speed_limit();
    *max_speed_limit = msg->max_speed_limit();
  }
}
bag.close();

Cyber RT也提供了類似功能，只不過ROS操作的是.bag文件，而Cyber RT操作的是.record文件，示例代碼如下：

RecordReader reader(readfile);
RecordMessage message;
uint64_t msg_count = reader.GetMessageNumber(CHANNEL_NAME_1);
AINFO << "MSGTYPE: " << reader.GetMessageType(CHANNEL_NAME_1);
AINFO << "MSGDESC: " << reader.GetProtoDesc(CHANNEL_NAME_1);

// read all message
uint64_t i = 0;
uint64_t valid = 0;
for (i = 0; i < msg_count; ++i) {
  if (reader.ReadMessage(&message)) {
    AINFO << "msg[" << i << "]-> "
          << "channel name: " << message.channel_name
          << "; content: " << message.content << "; msg time: " << message.time;
    valid++;
  } else {
    AERROR << "read msg[" << i << "] failed";
  }
}
AINFO << "static msg=================";
AINFO << "MSG validmsg:totalcount: " << valid << ":" << msg_count;

上述代碼位於/apollo/cyber/examples/record.cc文件中，其他接口可通過/apollo/cyber/record目錄下的record_reader.h和record_viewer.h文件查詢。