Apollo學習筆記（一）：canbus模組與車輛底盤之間的CAN數據傳輸過程
- 2019 年 11 月 2 日
- 筆記
Apollo學習筆記（一）：canbus模組與車輛底盤之間的CAN數據傳輸過程
部落客現在從車載自組網信道分配和多跳路由轉向了自動駕駛，沒啥經驗，想快些做出來個Demo還是得站在巨人的肩膀上才行，我選擇了Apollo，主要還是支援國產而且它的開發者套件有現成的底盤可以直接跑起來，但是apollo系統結構比較複雜，各種花哨的設計模式（消息適配器、工廠模式等）繞得人頭暈。日本那裡有個autoware是基於原生ROS的，也用Apollo開發者套件跑了下，就是普通的機器人開發那套，難度適合學生用來做項目，還是得師夷長技以制夷，後面繼續深入研究一下。
這次的學習是基於Apollo3.0的，因為3.0還是基於ROS的，後期研究autoware開發自己的系統能用得上，而且那個開發者套件也要用3.0。
canbus模組啟動過程
參考知行合一2018大佬的Apollo Planning模組源程式碼分析可以知道canbus模組的主入口為modules/canbus/main.cc：
APOLLO_MAIN(apollo::canbus::Canbus); 該宏展開後為：
1 #define APOLLO_MAIN(APP) 2 int main(int argc, char **argv) { 3 google::InitGoogleLogging(argv[0]); 4 google::ParseCommandLineFlags(&argc, &argv, true); 5 signal(SIGINT, apollo::common::apollo_app_sigint_handler); 6 APP apollo_app_; 7 ros::init(argc, argv, apollo_app_.Name()); 8 apollo_app_.Spin(); 9 return 0; 10 }
這裡直接引用知行合一2018大神對於Apollo Planning模組的分析：
Main函數完成以下工作：始化Google日誌工具，使用Google命令行解析工具解析相關參數，註冊接收中止訊號“SIGINT”的處理函數：apollo::common::apollo_app_sigint_handler（該函數的功能十分簡單，就是收到中止訊號“SIGINT”後，調用ros::shutdown()關閉ROS），創建apollo::planning::Planning對象：apollo_app_，初始化ROS環境，調用apollo_app_.Spin()函數開始消息處理循環。
————————————————
版權聲明：本文為CSDN部落客「知行合一2018」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/davidhopper/article/details/79176505
更詳細的不再贅述，大家可以去上面鏈接學習。
在apollo_app_.Spin();函數內會依次調用modules/canbus/canbus.cc的Init()和Start()。
Init()主要過程源碼分析
1 Status Canbus::Init() { 2 AdapterManager::Init(FLAGS_canbus_adapter_config_filename);/*完成AdapterManager的初始化， 3 FLAGS_canbus_adapter_config_filename對應於modules/canbus/common/canbus_gflags.cc中的 4 DEFINE_string(canbus_adapter_config_filename, "modules/canbus/conf/adapter.conf", 5 "The adapter config file"); 6 adapter.conf中配置了canbus模組訂閱和發布的topic 7 如果改成原生ROS的話，這裡的AdapterManager配置刪掉，改成ROS的topic訂閱和發布*/ 8 AINFO << "The adapter manager is successfully initialized."; 9 10 // load conf 11 //導入配置文件modules/canbus/conf/canbus_conf.pb.txt 12 /* 13 vehicle_parameter { 14 brand: LINCOLN_MKZ//指定車輛，後面生成對應的vehicle_factory，進而生成對應的message_manager_ 15 max_enable_fail_attempt: 5 16 driving_mode: COMPLETE_AUTO_DRIVE 17 } 18 19 can_card_parameter { 20 brand: ESD_CAN//指定車輛，後面生成對應的can_client_ 21 type: PCI_CARD 22 channel_id: CHANNEL_ID_ZERO 23 } 24 25 enable_debug_mode: false 26 enable_receiver_log: false 27 enable_sender_log: false 28 */ 29 //如果改成原生ROS的話此處也可以刪除，工廠模式也可以放一邊，上面導入的配置文件就是用來生成具體的產品工廠 30 //對象和產品對象，我們直接用ROS跑自己的工程一般車輛和CAN卡是固定的，可以改動後直接生成對應的產品對象 31 if (!common::util::GetProtoFromFile(FLAGS_canbus_conf_file, &canbus_conf_)) { 32 return OnError("Unable to load canbus conf file: " + 33 FLAGS_canbus_conf_file); 34 } 35 36 AINFO << "The canbus conf file is loaded: " << FLAGS_canbus_conf_file; 37 ADEBUG << "Canbus_conf:" << canbus_conf_.ShortDebugString(); 38 39 // Init can client 40 auto *can_factory = CanClientFactory::instance(); 41 can_factory->RegisterCanClients(); 42 can_client_ = can_factory->CreateCANClient(canbus_conf_.can_card_parameter()); 43 /* 44 std::unique_ptr<CanClient> CanClientFactory::CreateCANClient( 45 const CANCardParameter& parameter) { 46 auto factory = CreateObject(parameter.brand());//這裡確定了新建的can client對象， 47 //此處新建的是ESD CAN卡對應的can client對象（ESD CAN卡是開發者套件用的CAN卡），具體為 48 //modules/drivers/canbus/can_client/esd/esd_can_client.cc的對象。 49 //由此也可以看出modules/canbus/與modules/drivers/canbus/之間的聯繫是很緊密的， 50 //modules/canbus/偏向上層的針對不同車輛的針對性數據解析 51 //modules/drivers/canbus/偏向底層的一些共性功能的實現，比如 52 //modules/drivers/canbus/can_comm/message_manager就是 53 //modules/canbus/vehicle/lincoln/lincoln_message_manager的父類 54 //改成原生ROS時這兩個文件夾可以合到一起 55 if (!factory) { 56 AERROR << "Failed to create CAN client with parameter: " 57 << parameter.DebugString(); 58 } else if (!factory->Init(parameter)) { 59 AERROR << "Failed to initialize CAN card with parameter: " 60 << parameter.DebugString(); 61 } 62 return factory; 63 } 64 */ 65 if (!can_client_) { 66 return OnError("Failed to create can client."); 67 } 68 AINFO << "Can client is successfully created."; 69 70 VehicleFactory vehicle_factory; 71 vehicle_factory.RegisterVehicleFactory(); 72 auto vehicle_object = 73 vehicle_factory.CreateVehicle(canbus_conf_.vehicle_parameter());//類似地生成具體的車輛對象 74 if (!vehicle_object) { 75 return OnError("Failed to create vehicle:"); 76 } 77 78 message_manager_ = vehicle_object->CreateMessageManager();//生成對應車輛的message_manager_ 79 if (message_manager_ == nullptr) { 80 return OnError("Failed to create message manager."); 81 } 82 AINFO << "Message manager is successfully created."; 83 //初始化can_receiver_，就是將can_client_、message_manager_賦給can_receiver_的成員變數 84 if (can_receiver_.Init(can_client_.get(), message_manager_.get(), 85 canbus_conf_.enable_receiver_log()) != ErrorCode::OK) { 86 return OnError("Failed to init can receiver."); 87 } 88 AINFO << "The can receiver is successfully initialized."; 89 //初始化can_sender_，就是將can_client_賦給can_sender_的成員變數 90 if (can_sender_.Init(can_client_.get(), canbus_conf_.enable_sender_log()) != 91 ErrorCode::OK) { 92 return OnError("Failed to init can sender."); 93 } 94 AINFO << "The can sender is successfully initialized."; 95 96 //生成對應車輛的vehicle_controller_ 97 //至此vehicle_object生成了message_manager_和vehicle_controller_ 98 //message_manager_ 用來解析canbus從CAN接收到的消息 99 //vehicle_controller_ 用來更新canbus發往CAN的數據對象，也叫作協議類型數據 100 //（如剎車這個命令（協議）對應的對象，apollo將每種命令建了個類，比如林肯的剎車對應的類是 101 //modules/canbus/vehicle/lincoln/protocol/brake_60.h）， 102 //也就是canbus接收到上層control的commond後通過vehicle_controller_ 103 //更新協議類型數據內的具體成員變數（如剎車這個命令對應的對象內的“剎車量pedal_cmd_”這一成員變數） 104 vehicle_controller_ = vehicle_object->CreateVehicleController(); 105 if (vehicle_controller_ == nullptr) { 106 return OnError("Failed to create vehicle controller."); 107 } 108 AINFO << "The vehicle controller is successfully created."; 109 110 /*vehicle_controller_->Init(...) （實際上是 111 modules/canbus/vehicle/lincoln/lincoln_controller.cc中的LincolnController::Init）裡面 112 先生成brake_60_等協議類型數據，接著通過 113 can_sender_->AddMessage(Brake60::ID, brake_60_, false);等將brake_60_等協議類型數據 114 加入can_sender_的成員變數send_messages_（向量），後面can_sender_發送數據時就會將send_messages_ 115 中的各個協議類型數據包含的CAN幀can_frame_to_send_發送到底盤 116 */ 117 if (vehicle_controller_->Init(canbus_conf_.vehicle_parameter(), &can_sender_, 118 message_manager_.get()) != ErrorCode::OK) { 119 return OnError("Failed to init vehicle controller."); 120 } 121 AINFO << "The vehicle controller is successfully initialized."; 122 123 CHECK(AdapterManager::GetControlCommand()) << "Control is not initialized."; 124 CHECK(AdapterManager::GetGuardian()) << "Guardian is not initialized."; 125 // TODO(QiL) : depreacte this 126 //類似原生ROS的添加回調函數，Apollo將topic的發布和訂閱都集成到了AdapterManager進行統一管理 127 //Canbus::OnControlCommand就是canbus模組對上層cotrol_command的回調函數 128 if (!FLAGS_receive_guardian) { 129 AdapterManager::AddControlCommandCallback(&Canbus::OnControlCommand, this); 130 } else { 131 AdapterManager::AddGuardianCallback(&Canbus::OnGuardianCommand, this); 132 } 133 134 return Status::OK(); 135 }
Init()主要過程流程圖（為方便整合不嚴格按照程式碼里的順序）
Start()主要過程源碼分析
1 /*Init()生成的對象有can_client_ , vehicle_object , message_manager_, 2 vehicle_controller_ , can_receiver_ 和 can_sender_， 3 Start()裡面就是調用can_client_, can_receiver_, can_sender_和vehicle_controller_的Start()將 4 它們的功能各自啟動起來，比如can_client_ 的Start() （實際上是 5 modules/drivers/canbus/can_client/esd/esd_can_client.cc的Start()） 6 就是調用third_party/can_card_library/esd_can/include/ntcan.h的內置函數canOpen()等設置埠等 7 以啟動CAN卡，ntcan.h是買CAN卡的時候附帶光碟里的文件，買了開發者套件裝好CAN卡以後要把這個文件拷到 8 third_party/can_card_library/esd_can/include/下使用。因為ntcan.h是花錢買的所以就不把ntcan.h 9 的程式碼放上來了。 10 11 最後啟動定時器循環運行Canbus::OnTimer，OnTimer這個函數就是發布底盤資訊用的，發布以後 12 訂閱底盤topic的上層模組就能接收到底盤資訊。 13 */ 14 Status Canbus::Start() { 15 // 1. init and start the can card hardware 16 if (can_client_->Start() != ErrorCode::OK) { 17 return OnError("Failed to start can client"); 18 } 19 AINFO << "Can client is started."; 20 21 // 2. start receive first then send 22 if (can_receiver_.Start() != ErrorCode::OK) { 23 return OnError("Failed to start can receiver."); 24 } 25 AINFO << "Can receiver is started."; 26 27 // 3. start send 28 if (can_sender_.Start() != ErrorCode::OK) { 29 return OnError("Failed to start can sender."); 30 } 31 32 // 4. start controller 33 if (vehicle_controller_->Start() == false) { 34 return OnError("Failed to start vehicle controller."); 35 } 36 37 // 5. set timer to triger publish info periodly 38 const double duration = 1.0 / FLAGS_chassis_freq; 39 timer_ = AdapterManager::CreateTimer(ros::Duration(duration), 40 &Canbus::OnTimer, this); 41 42 // last step: publish monitor messages 43 apollo::common::monitor::MonitorLogBuffer buffer(&monitor_logger_); 44 buffer.INFO("Canbus is started."); 45 46 return Status::OK(); 47 }
Start()主要過程流程圖
下面分canbus模組向底盤發送數據和canbus模組從底盤接收數據兩部分進行深入分析。
canbus模組向底盤發送數據
主要過程
canbus模組向底盤發送數據的開端在canbus模組接收到上層control_command，因此Canbus::OnControlCommand是發送的開端。
1 void Canbus::OnControlCommand(const ControlCommand &control_command) { 2 int64_t current_timestamp = 3 apollo::common::time::AsInt64<common::time::micros>(Clock::Now()); 4 // if command coming too soon, just ignore it. 5 if (current_timestamp - last_timestamp_ < FLAGS_min_cmd_interval * 1000) { 6 ADEBUG << "Control command comes too soon. Ignore.n Required " 7 "FLAGS_min_cmd_interval[" 8 << FLAGS_min_cmd_interval << "], actual time interval[" 9 << current_timestamp - last_timestamp_ << "]."; 10 return; 11 } 12 13 last_timestamp_ = current_timestamp; 14 ADEBUG << "Control_sequence_number:" 15 << control_command.header().sequence_num() << ", Time_of_delay:" 16 << current_timestamp - control_command.header().timestamp_sec(); 17 18 /*從此處開始的vehicle_controller_->Update(control_command)和can_sender_.Update()是關鍵 19 正如之前所提到的，vehicle_controller_->Update(control_command)用來更新協議類型數據的 20 成員變數，接著can_sender_.Update()把更新之後的數據通過can_frame_to_update_ 21 賦值給can_frame_to_send_，can_sender_在Start()是開啟的發送執行緒將can_frame_to_send_ 22 通過CAN卡附帶驅動內的canWrite()函數注入底盤CAN網路 23 24 這兩個Update()涉及的函數比較多，中間挺多跳轉，就不放源碼了，跳轉的過程在後面流程圖中給出，具體程式碼的 25 實現大家還是需要自己去看程式碼 26 */ 27 if (vehicle_controller_->Update(control_command) != ErrorCode::OK) { 28 AERROR << "Failed to process callback function OnControlCommand because " 29 "vehicle_controller_->Update error."; 30 return; 31 } 32 can_sender_.Update(); 33 }
vehicle_controller_->Update流程圖（以剎車對應的協議類型數據為例）
can_sender_.Update()流程圖（以剎車對應的協議類型數據為例）
canbus模組從底盤接收數據
主要過程
在canbus Start()的時候，can_receiver_.Start()啟動了從底盤接收數據的執行緒，執行緒內運行CanReceiver::RecvThreadFunc()
1 // 2. start receive first then send 2 if (can_receiver_.Start() != ErrorCode::OK) { 3 return OnError("Failed to start can receiver."); 4 } 5 AINFO << "Can receiver is started."; 6 7 template <typename SensorType> 8 ::apollo::common::ErrorCode CanReceiver<SensorType>::Start() { 9 if (is_init_ == false) { 10 return ::apollo::common::ErrorCode::CANBUS_ERROR; 11 } 12 is_running_ = true; 13 14 thread_.reset(new std::thread([this] { RecvThreadFunc(); })); 15 if (thread_ == nullptr) { 16 AERROR << "Unable to create can client receiver thread."; 17 return ::apollo::common::ErrorCode::CANBUS_ERROR; 18 } 19 return ::apollo::common::ErrorCode::OK; 20 }
RecvThreadFunc()內循環運行can_client_->Receive(&buf, &frame_num)
1 template <typename SensorType> 2 void CanReceiver<SensorType>::RecvThreadFunc() { 3 AINFO << "Can client receiver thread starts."; 4 CHECK_NOTNULL(can_client_); 5 CHECK_NOTNULL(pt_manager_); 6 7 int32_t receive_error_count = 0; 8 int32_t receive_none_count = 0; 9 const int32_t ERROR_COUNT_MAX = 10; 10 std::chrono::duration<double, std::micro> default_period{10 * 1000}; 11 12 while (IsRunning()) { 13 std::vector<CanFrame> buf; 14 int32_t frame_num = MAX_CAN_RECV_FRAME_LEN; 15 if (can_client_->Receive(&buf, &frame_num) != 16 ::apollo::common::ErrorCode::OK) { 17 /* 18 can_client_為modules/drivers/canbus/can_client/esd/esd_can_client.cc的實例化對象， 19 其Receive()函數調用了third_party/can_card_library/esd_can/include/ntcan.h的canRead函數從 20 CAN網路中讀取數據並存入buf， 21 const int32_t ret = canRead(dev_handler_, recv_frames_, frame_num, nullptr); 22 buf的定義是std::vector<CanFrame> buf; 23 CanFrame在之前can_sender_.Update()流程圖內有分析。 24 */ 25 LOG_IF_EVERY_N(ERROR, receive_error_count++ > ERROR_COUNT_MAX, 26 ERROR_COUNT_MAX) 27 << "Received " << receive_error_count << " error messages."; 28 std::this_thread::sleep_for(default_period); 29 continue; 30 } 31 receive_error_count = 0; 32 33 if (buf.size() != static_cast<size_t>(frame_num)) { 34 AERROR_EVERY(100) << "Receiver buf size [" << buf.size() 35 << "] does not match can_client returned length[" 36 << frame_num << "]."; 37 } 38 39 if (frame_num == 0) { 40 LOG_IF_EVERY_N(ERROR, receive_none_count++ > ERROR_COUNT_MAX, 41 ERROR_COUNT_MAX) 42 << "Received " << receive_none_count << " empty messages."; 43 std::this_thread::sleep_for(default_period); 44 continue; 45 } 46 receive_none_count = 0; 47 48 for (const auto &frame : buf) { 49 uint8_t len = frame.len; 50 uint32_t uid = frame.id; 51 const uint8_t *data = frame.data; 52 pt_manager_->Parse(uid, data, len); 53 /* 54 pt_manager_在Init時被賦值為message_manager_（modules/canbus/vehicle/lincoln 55 /lincoln_message_manager.cc的實例化對象），message_manager_用來解析從CAN網路獲取的CanFrame 56 */ 57 if (enable_log_) { 58 ADEBUG << "recv_can_frame#" << frame.CanFrameString(); 59 } 60 } 61 std::this_thread::yield(); 62 } 63 AINFO << "Can client receiver thread stopped."; 64 }
接收CAN數據流程圖
至於chassis_datail找不到的問題大家可以從https://blog.csdn.net/M_Alan_walker/article/details/88823610的最後面獲得解答。
後記
部落客主要是從https://blog.csdn.net/M_Alan_walker/article/details/88823610學習的，加入一些自己的理解，有些分析得不完善的地方大家在上面的網址可以得到更詳細的補充。
