上个月换了工做,公司使用的开发软件平台也不同了,如今主要使用 Apollo ,与 ROS 系统区别仍是蛮大的,因此下面这一系列文章都是在百度 Apollo 系统下开发控制模块的记录。
由于,车身自己的控制接口是 can 通信, 因此拿到手的第一件工做,就是把上层控制与车身底盘的通信模块开发出来。
So,须要从canbus模块入手。html
首先上一下Apollo的文件结构,node
如上图所示,canbus模块与 driver 模块以及 control 模块之间的消息传递方式如上图所示,下面,开始详细的描述,如何在 canbus 中添加一辆车的底层协议解析过程。web
简单的对canbus框架进行了介绍以后,后面就开始进行,子模块的详细介绍。算法
common模块主要有两个文件:canbus_gflags.h canbus_gflags.cc
具体内容上图:
canbus_gflags.hdocker
#pragma once #include "gflags/gflags.h" // System gflags DECLARE_string(canbus_node_name); DECLARE_string(canbus_module_name); // data file DECLARE_string(canbus_conf_file); // Canbus gflags DECLARE_double(chassis_freq); DECLARE_int64(min_cmd_interval); // chassis_detail message publish DECLARE_bool(enable_chassis_detail_pub); // canbus test files DECLARE_string(canbus_test_file); // canbus test files DECLARE_bool(receive_guardian); DECLARE_int32(guardian_cmd_pending_queue_size); DECLARE_int32(control_cmd_pending_queue_size);
canbus_gflags.ccexpress
#include "modules/canbus/common/canbus_gflags.h" // System gflags DEFINE_string(canbus_node_name, "chassis", "The chassis module name in proto"); DEFINE_string(canbus_module_name, "canbus_component", "Module name"); // data file DEFINE_string(canbus_conf_file, "/apollo/modules/canbus/conf/canbus_conf.pb.txt", "Default canbus conf file"); // Canbus gflags DEFINE_double(chassis_freq, 100, "Chassis feedback timer frequency."); DEFINE_int64(min_cmd_interval, 5, "Minimum control command interval in ms."); // chassis_detail message publish DEFINE_bool(enable_chassis_detail_pub, false, "Chassis Detail message publish"); // canbus test files DEFINE_string(canbus_test_file, "/apollo/modules/canbus/testdata/canbus_test.pb.txt", "canbus tester input test file, in ControlCommand pb format."); // enable receiving guardian command // TODO(QiL) : depreciate this after test DEFINE_bool(receive_guardian, false, "Enable receiving guardian message on canbus side"); DEFINE_int32(guardian_cmd_pending_queue_size, 10, "Max guardian cmd pending queue size"); DEFINE_int32(control_cmd_pending_queue_size, 10, "Max control cmd pending queue size");
这两个文件主要是,对整个canbus模块内须要用到的一些标志位和变量配置,进行声明和初始化。
使用了 Google 的 gflags 功能,具体我也不是很明白,这里不求甚解,会用就行,想详细了解的能够去官网查看一下gflags的具体用法。apache
DECLARE_xxx 就是对一些变量进行声明,以方便在其余函数中对这个变量进行调用,通常有 DECLARE_bool, DECLARE_string, DECLARE_int32, DECLARE_int64, DECLARE_double等。
DEFINE_xxx(name, val, help_txt)初始化这边变量的初始值,及变量说明。安全
这个子模块下,主要是canbus模块的一些具体的配置。
划重点了,canbus_conf.pb.txt 是对 canbus/proto 文件夹下 canbus_conf.proto 中的变量进行配置。canbus.conf 是对 canbus/common 文件夹下 canbus_gflags.h 中 declare 的变量进行配置。架构
canbus_conf.pb.txtapp
vehicle_parameter { brand: LINCOLN_MKZ // 车辆类型设定 max_enable_fail_attempt: 5 // 最大失败次数 driving_mode: COMPLETE_AUTO_DRIVE // 初始车辆模式 } can_card_parameter { brand: ESD_CAN // can卡设定 type: PCI_CARD // can卡类型 channel_id: CHANNEL_ID_ZERO // can 通道设定 interface: NATIVE // can接口设定 } enable_debug_mode: false // 不使用 debug 模式 enable_receiver_log: false // 不使用 receiver log enable_sender_log: false // 不适用 sender log
canbus.conf
// 配置全局变量,以及canbus --flagfile=/apollo/modules/common/data/global_flagfile.txt --canbus_conf_file=/apollo/modules/canbus/conf/canbus_conf.pb.txt // 对 common 中 declare 的变量进行设置 --noenable_chassis_detail_pub --noreceive_guardian
module_config { module_library : "/apollo/bazel-bin/modules/canbus/libcanbus.so" timer_components { class_name : "CanbusComponent" config { name: "canbus" config_file_path: "/apollo/modules/canbus/conf/canbus_conf.pb.txt" flag_file_path: "/apollo/modules/canbus/conf/canbus.conf" interval: 10 } } }
dag文件我也是第一次见到,下面都是我的的理解
module_library : 编译生成的可执行文件,启动此模块就是调用这个文件
timer_components : 定时器模块配置
class_name : 定时器中设定的类名称
config : 定时器的一些配置,名称,配置文件的路径,flag文件路径,以及interval,这个应该是此模块调用的频时间间隔 10ms,对应 canbus_gflags.cc 中 chassis_freq 频率100Hz 。
模块启动文件,也就是把 dag文件路径放进来。
<cyber> <module> <name>canbus</name> <dag_conf>/apollo/modules/canbus/dag/canbus.dag</dag_conf> <process_name>canbus</process_name> </module> </cyber>
proto文件夹,里面主要是一些protocol buffer文件,具体的语法见官方文档
https://developers.google.cn/protocol-buffers
这里就不详细解释了。
里面主要有,
在 canbus 模块启动的时候,会载入 canbus.conf 文件里的配置信息,
--flagfile=/apollo/modules/common/data/global_flagfile.txt --canbus_conf_file=/apollo/modules/canbus/conf/canbus_conf.pb.txt --noenable_chassis_detail_pub --noreceive_guardian
canbus_conf.pb.txt 里面的配置信息
vehicle_parameter { brand: LINCOLN_MKZ max_enable_fail_attempt: 5 driving_mode: COMPLETE_AUTO_DRIVE } can_card_parameter { brand: ESD_CAN type: PCI_CARD channel_id: CHANNEL_ID_ZERO interface: NATIVE } enable_debug_mode: false enable_receiver_log: false enable_sender_log: false
testdata 文件夹下是 canbus_test.pb.txt 和 conf 文件夹,canbus_test.pb.txt 文件里面是按照 Control Command 格式设定的 control_cmd,conf文件夹里面就是和canbus文件夹下的canbus_conf.pb.txt功能一致。
tools这个部分主要是用来对开发好的 canbus 模块进行相关测试,测试方式有两种,一个是canbus_tester ,另外一个是 teletop 。
vehicle文件夹的结构以下
$vehicle:这个文件夹下,主要是对车身底盘can报文解析的具体操做,protocol文件夹下,会分别对读取的底盘信息,以及下发的控制指令进行相应的位操做,对应
canbus/proto 下对应的 $vehicle.proto 文件, $vehicle_vehicle_factory 中继承
abstract_vehicle_factory类,建立了两个指针,一个指向 $vehicle_controller ,一个指向 $vehicle_message_manager。 $vehicle_controller 中会对canbus读取到的报文进行解析成chassis信息,而后pub出去,另外 $vehicle_controller 中还会对ControlCommand 下发的控制指令转换成 can 报文,经过 can_sender_ 指针经过can卡下发到车身。 $vehicle_message_manager 中会设定须要接受的报文ID,和下发的报文ID。
abstract_vehicle_factory:读取车辆参数,具体配置参数在 canbus/conf 中的canbus_conf.pb.txt 中。$vehicle_vehicle_factory 中的具体车辆类会继承此类。
vehicle_factory:这部分主要是在增长新的车辆的时候,须要在此对车辆进行注册。
vehicle_controller:定义 vehicle_controller 基类,后面须要添加不一样车辆信息的时候,须要对此基类继承。对车辆驾驶模式以及各个控制指令在此处进行 update。
canbus 模块下根目录主要是 canbus_component 和 README 文件,README 没什么好说的。
canbus_component 中,会对 canbus 模块进行初始化,下面是初始化的步骤,具体函数在 bool CanbusComponent::Init() 中:
初始化完成后,这个模块会主要执行两个功能函数 Proc() 和 OnControlCommand()。
Proc() 函数会按照 canbus.dag 中设置的 interval 值去周期性的调用这个函数,interval 单位为 ms 。
OnControlCommand() 会在canbus 模块每次收到 ControlCommand 时被调用,pub 出对车辆的控制指令。
以前一直都用ROS开发,如今换到Apollo系统,二者的差异真的蛮大的,ROS的每一个包之间互相依赖的并很少,而Apollo使用了大量的指针,在各个module中指来指去,代码阅读起来很不方便,可是Apollo的安全监控作的仍是很好的。
为了之后再次使用这个模块的时候,避免开发过程当中再次踩到坑,最后,再次梳理一下canbus整个模块。
首先,canbus模块会pub 出去的消息有 chassis,chassis_details(通常不pub,可设置),须要读取的消息有 ControlCommand ,以及底盘上发的can报文。整个canbus模块的初始化,在上面有详细的描述了。
而后,这里在清晰的按照每条线理一下,数据传输的过程
drivers/can_common/message_manager.h 中的
GetSensorData(SensorType*const sensor_data) 获取到底盘上发的 can
报文(具体的底盘can报文如何解析,以及传送到message_manager,这部分,在后面的
driver中有介绍,这里就不赘述了),$vehicle_controller 中的 chassis
函数,会将获取到的底盘报文最终解析成chassis ,而后pub出去
从ControlCommand 接收到控制指令后,会调用 vehicle_controller 中的 update
函数,将控制命令,转换成对用的 can 报文,在经过 can_sender->update(),将can报文下发出去
最后,记录一下,在Apollo中,添加新的车辆的通常步骤:
在 common/configs/proto/vehicle_config.proto 文件中的 VehicleBrand
中添加须要增长的车型
在 canbus/proto/文件夹下,添加所需的车辆信息的 proto 文件
在 canbus/proto/chassis_detail.proto 的 chassis_detail 中添加车辆
在 canbus/vehicle 中新建添加车辆文件夹,文件夹下包含 protocol
文件夹以及车辆对应的controller,message_manager,vehicle_factory
在modules/canbus/vehicle/vehicle_factory.cc里注册新的车辆
在 modules/canbus/conf/canbus_conf.pb.tx中更新配置,主要是对应的车辆和can卡,can卡这一块的配置,在后面的文章中再进行记录,暂时没看
在modules/common/data/vehicle_param.pb.txt brand中 添加新增的车辆
下面主要结合代码部分,对canbus模块进行说明,先从最重要的 canbus_component 开始:
#pragma once #include <memory> #include <string> #include <utility> #include <vector> #include "cyber/common/macros.h" #include "cyber/component/timer_component.h" #include "cyber/cyber.h" #include "cyber/timer/timer.h" #include "modules/canbus/proto/chassis_detail.pb.h" #include "modules/drivers/canbus/proto/can_card_parameter.pb.h" #include "modules/guardian/proto/guardian.pb.h" #include "modules/canbus/vehicle/vehicle_controller.h" #include "modules/common/monitor_log/monitor_log_buffer.h" #include "modules/common/status/status.h" #include "modules/control/proto/control_cmd.pb.h" #include "modules/drivers/canbus/can_client/can_client.h" #include "modules/drivers/canbus/can_comm/can_receiver.h" #include "modules/drivers/canbus/can_comm/can_sender.h" #include "modules/drivers/canbus/can_comm/message_manager.h" namespace apollo { namespace canbus { class CanbusComponent final : public apollo::cyber::TimerComponent { public: CanbusComponent(); // 构造函数 std::string Name() const; // 模块名 private: bool Init() override; // 对整个canbus模块进行配置,具体配置见上文 bool Proc() override; // Cyber 会按照 dag 文件中设置的周期调用此函数 void Clear() override; // 清空此模块的相关配置 void PublishChassis(); // pub chassis void PublishChassisDetail(); // pub chassis_detail void OnControlCommand(const apollo::control::ControlCommand &control_command); void OnGuardianCommand(const apollo::guardian::GuardianCommand &guardian_command); // 两个回调函数,根据配置进行选择 apollo::common::Status OnError(const std::string &error_msg); // 对车辆底盘状态进行监控 void RegisterCanClients(); // 注册can client CanbusConf canbus_conf_; std::shared_ptr<cyber::Reader<apollo::guardian::GuardianCommand>> guardian_cmd_reader_; std::shared_ptr<cyber::Reader<apollo::control::ControlCommand>> control_command_reader_; std::unique_ptr<apollo::drivers::canbus::CanClient> can_client_; CanSender<ChassisDetail> can_sender_; apollo::drivers::canbus::CanReceiver<ChassisDetail> can_receiver_; std::unique_ptr<MessageManager<ChassisDetail>> message_manager_; std::unique_ptr<VehicleController> vehicle_controller_; int64_t last_timestamp_ = 0; ::apollo::common::monitor::MonitorLogBuffer monitor_logger_buffer_; std::shared_ptr<cyber::Writer<Chassis>> chassis_writer_; std::shared_ptr<cyber::Writer<ChassisDetail>> chassis_detail_writer_; }; CYBER_REGISTER_COMPONENT(CanbusComponent) } // namespace canbus } // namespace apollo
CanbusComponent 文件是整个canbus模块的最核心部分,这部分代码会对整个模块进行相关的初始化和配置,主要包括can卡模块的配置,车辆底盘上发信息的解析发布,对控制命令的转译下发等,对应的函数具体内容能够本身看一下相关的 .cc 文件,里面没啥复杂的计算和操做,在这就不详细的解释了。
打开这个文件夹,能够看到里面有不少个以车辆型号命名的文件夹,以及其余几个文件 abstract_vehicle_factory, vehicle_controller, vehicle_factory。
下面就开始一一对其进行详细介绍:
首先上代码:
abstract_vehicle_factory.h
/****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ /** * @file */ #pragma once #include <memory> #include "modules/canbus/proto/chassis_detail.pb.h" #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus/vehicle/vehicle_controller.h" #include "modules/drivers/canbus/can_comm/message_manager.h" /** * @namespace apollo::canbus * @brief apollo::canbus */ namespace apollo { namespace canbus { /** * @class AbstractVehicleFactory * * @brief this class is the abstract factory following the AbstractFactory * design pattern. It can create VehicleController and MessageManager based on * a given VehicleParameter. */ class AbstractVehicleFactory { public: /** * @brief destructor */ virtual ~AbstractVehicleFactory() = default; /** * @brief the interface of creating a VehicleController class * @returns a unique pointer that points to the created VehicleController * object. */ virtual std::unique_ptr<VehicleController> CreateVehicleController() = 0; /** * @brief the interface of creating a MessageManager class * @returns a unique pointer that points to the created MessageManager object. */ virtual std::unique_ptr<MessageManager<ChassisDetail>> CreateMessageManager() = 0; /** * @brief set VehicleParameter. */ void SetVehicleParameter(const VehicleParameter &vehicle_paramter); private: VehicleParameter vehicle_parameter_; }; } // namespace canbus } // namespace apollo
abstract_vehicle_factory.cc
/****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ #include "modules/canbus/vehicle/abstract_vehicle_factory.h" namespace apollo { namespace canbus { void AbstractVehicleFactory::SetVehicleParameter(const VehicleParameter &vehicle_parameter) { vehicle_parameter_ = vehicle_parameter; } } // namespace canbus } // namespace apollo
抽象化车辆工厂,Apollo是按照工厂模式开发的,这里主要功能是根据 vehicle_parameter_ 对车辆进行初始化,以及建立两个指针:
其中的虚函数,会在vehicle_factory中有具体的使用。
vehicle_factory.h
/****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ /** * @file */ #pragma once #include <memory> #include "modules/canbus/vehicle/abstract_vehicle_factory.h" #include "modules/common/util/factory.h" /** * @namespace apollo::canbus * @brief apollo::canbus */ namespace apollo { namespace canbus { /** * @class VehicleFactory * * @brief This class is a factory class that will generate different * vehicle factories based on the vehicle brand. */ class VehicleFactory : public common::util::Factory<apollo::common::VehicleBrand, AbstractVehicleFactory> { public: /** * @brief register supported vehicle factories. */ void RegisterVehicleFactory(); /** * @brief Creates an AbstractVehicleFactory object based on vehicle_parameter * @param vehicle_parameter is defined in vehicle_parameter.proto */ std::unique_ptr<AbstractVehicleFactory> CreateVehicle( const VehicleParameter &vehicle_parameter); }; } // namespace canbus } // namespace apollo
vehicle_factory.cc
/****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ #include "modules/canbus/vehicle/vehicle_factory.h" #include "modules/canbus/proto/vehicle_parameter.pb.h" #include "modules/canbus/vehicle/ch/ch_vehicle_factory.h" #include "modules/canbus/vehicle/ge3/ge3_vehicle_factory.h" #include "modules/canbus/vehicle/gem/gem_vehicle_factory.h" #include "modules/canbus/vehicle/lexus/lexus_vehicle_factory.h" #include "modules/canbus/vehicle/lincoln/lincoln_vehicle_factory.h" #include "modules/canbus/vehicle/transit/transit_vehicle_factory.h" #include "modules/canbus/vehicle/wey/wey_vehicle_factory.h" #include "modules/canbus/vehicle/zhongyun/zhongyun_vehicle_factory.h" namespace apollo { namespace canbus { void VehicleFactory::RegisterVehicleFactory() { Register(apollo::common::LINCOLN_MKZ, []() -> AbstractVehicleFactory * { return new LincolnVehicleFactory(); }); Register(apollo::common::GEM, []() -> AbstractVehicleFactory * { return new GemVehicleFactory(); }); Register(apollo::common::LEXUS, []() -> AbstractVehicleFactory * { return new LexusVehicleFactory(); }); Register(apollo::common::TRANSIT, []() -> AbstractVehicleFactory * { return new TransitVehicleFactory(); }); Register(apollo::common::GE3, []() -> AbstractVehicleFactory * { return new Ge3VehicleFactory(); }); Register(apollo::common::WEY, []() -> AbstractVehicleFactory * { return new WeyVehicleFactory(); }); Register(apollo::common::ZHONGYUN, []() -> AbstractVehicleFactory * { return new ZhongyunVehicleFactory(); }); Register(apollo::common::CH, []() -> AbstractVehicleFactory * { return new ChVehicleFactory(); }); } std::unique_ptr<AbstractVehicleFactory> VehicleFactory::CreateVehicle(const VehicleParameter &vehicle_parameter) { auto abstract_factory = CreateObject(vehicle_parameter.brand()); if (!abstract_factory) { AERROR << "failed to create vehicle factory with " << vehicle_parameter.DebugString(); } else { abstract_factory->SetVehicleParameter(vehicle_parameter); AINFO << "successfully created vehicle factory with " << vehicle_parameter.DebugString(); } return abstract_factory; } } // namespace canbus } // namespace apollo
vehicle_factory会将须要添加的车辆进行注册,注册以后,就会将注册的对应的车辆型号与具体的车辆对应起来,举个栗子来讲,LINCOLN_MKZ 在注册后,经过配置 canbus_conf.pb.txt 中的 brand 参数就能够在canbus模块初始化时,新建一个 LincolnVehicleFactory(在LincolnVehicleFactory中,会生成对应的指针,controller 和 message_manager)
注意:brand信息是在common中的对应proto文件中添加相应的品牌,另外别忘了,要引入对应的vehicle_factory头文件。
vehicle_controller.h
/****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ /** * @file vehicle_controller.h * @brief The class of VehicleController */ #pragma once #include <unordered_map> #include "modules/canbus/proto/canbus_conf.pb.h" #include "modules/canbus/proto/chassis.pb.h" #include "modules/canbus/proto/chassis_detail.pb.h" #include "modules/control/proto/control_cmd.pb.h" #include "modules/common/configs/vehicle_config_helper.h" #include "modules/common/proto/error_code.pb.h" #include "modules/drivers/canbus/can_comm/can_sender.h" #include "modules/drivers/canbus/can_comm/message_manager.h" #include "modules/drivers/canbus/can_comm/protocol_data.h" /** * @namespace apollo::canbus * @brief apollo::canbus */ namespace apollo { namespace canbus { using ::apollo::drivers::canbus::CanSender; using ::apollo::drivers::canbus::MessageManager; /** * @class VehicleController * * @brief This is the interface class of vehicle controller. It defines pure * virtual functions, and also some implemented common functions. */ class VehicleController { public: virtual ~VehicleController() = default; /** * @brief initialize the vehicle controller. * @param can_sender a pointer to canbus sender. * @param message_manager a pointer to the message_manager. * @return error_code */ virtual common::ErrorCode Init( const VehicleParameter ¶ms, CanSender<ChassisDetail> *const can_sender, MessageManager<ChassisDetail> *const message_manager) = 0; /** * @brief start the vehicle controller. * @return true if successfully started. */ virtual bool Start() = 0; /** * @brief stop the vehicle controller. */ virtual void Stop() = 0; /** * @brief calculate and return the chassis. * @returns a copy of chassis. Use copy here to avoid multi-thread issues. */ virtual Chassis chassis() = 0; /** * @brief update the vehicle controller. * @param command the control command * @return error_code */ virtual common::ErrorCode Update(const control::ControlCommand &command); /** * @brief set vehicle to appointed driving mode. * @param driving mode to be appointed. * @return error_code */ virtual common::ErrorCode SetDrivingMode( const Chassis::DrivingMode &driving_mode); private: /* * @brief main logical function for operation the car enter or exit the auto * driving */ virtual void Emergency() = 0; virtual common::ErrorCode EnableAutoMode() = 0; virtual common::ErrorCode DisableAutoMode() = 0; virtual common::ErrorCode EnableSteeringOnlyMode() = 0; virtual common::ErrorCode EnableSpeedOnlyMode() = 0; /* * @brief NEUTRAL, REVERSE, DRIVE */ virtual void Gear(Chassis::GearPosition state) = 0; /* * @brief detail function for auto driving brake with new acceleration * acceleration:0.00~99.99, unit:% */ virtual void Brake(double acceleration) = 0; /* * @brief drive with old acceleration gas:0.00~99.99 unit:% */ virtual void Throttle(double throttle) = 0; /* * @brief drive with new acceleration/deceleration:-7.0~7.0, unit:m/s^2, * acc:-7.0~7.0, unit:m/s^2 */ virtual void Acceleration(double acc) = 0; /* * @brief steering with old angle speed angle:-99.99~0.00~99.99, unit:%, * left:+, right:- */ virtual void Steer(double angle) = 0; /* * @brief steering with new angle speed angle:-99.99~0.00~99.99, unit:%, * left:+, right:- angle_spd:0.00~99.99, unit:deg/s */ virtual void Steer(double angle, double angle_spd) = 0; /* * @brief set Electrical Park Brake */ virtual void SetEpbBreak(const control::ControlCommand &command) = 0; virtual void SetBeam(const control::ControlCommand &command) = 0; virtual void SetHorn(const control::ControlCommand &command) = 0; virtual void SetTurningSignal(const control::ControlCommand &command) = 0; virtual void SetLimits() {} protected: virtual Chassis::DrivingMode driving_mode(); virtual void set_driving_mode(const Chassis::DrivingMode &driving_mode); protected: canbus::VehicleParameter params_; common::VehicleParam vehicle_params_; CanSender<ChassisDetail> *can_sender_ = nullptr; MessageManager<ChassisDetail> *message_manager_ = nullptr; bool is_initialized_ = false; // own by derviative concrete controller Chassis::DrivingMode driving_mode_ = Chassis::COMPLETE_MANUAL; bool is_reset_ = false; // reset command from control command std::mutex mode_mutex_; // only use in this base class }; } // namespace canbus } // namespace apollo
vehicle_controller.cc
/****************************************************************************** * Copyright 2017 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ #include "modules/canbus/vehicle/vehicle_controller.h" #include "cyber/common/log.h" namespace apollo { namespace canbus { using common::ErrorCode; using control::ControlCommand; Chassis::DrivingMode VehicleController::driving_mode() { std::lock_guard<std::mutex> lock(mode_mutex_); return driving_mode_; } void VehicleController::set_driving_mode( const Chassis::DrivingMode &driving_mode) { std::lock_guard<std::mutex> lock(mode_mutex_); driving_mode_ = driving_mode; } ErrorCode VehicleController::SetDrivingMode( const Chassis::DrivingMode &driving_mode) { if (driving_mode == Chassis::EMERGENCY_MODE) { AINFO << "Can't set vehicle to EMERGENCY_MODE driving mode."; return ErrorCode::CANBUS_ERROR; } // vehicle in emergency mode only response to manual mode to reset. if (driving_mode_ == Chassis::EMERGENCY_MODE && driving_mode != Chassis::COMPLETE_MANUAL) { AINFO << "Vehicle in EMERGENCY_MODE, only response to COMPLETE_MANUAL mode."; AINFO << "Only response to RESET ACTION."; return ErrorCode::CANBUS_ERROR; } // if current mode is same as previous, no need to set. if (driving_mode_ == driving_mode) { return ErrorCode::OK; } switch (driving_mode) { case Chassis::COMPLETE_AUTO_DRIVE: { if (EnableAutoMode() != ErrorCode::OK) { AERROR << "Failed to enable auto mode."; return ErrorCode::CANBUS_ERROR; } break; } case Chassis::COMPLETE_MANUAL: { if (DisableAutoMode() != ErrorCode::OK) { AERROR << "Failed to disable auto mode."; return ErrorCode::CANBUS_ERROR; } break; } case Chassis::AUTO_STEER_ONLY: { if (EnableSteeringOnlyMode() != ErrorCode::OK) { AERROR << "Failed to enable steer only mode."; return ErrorCode::CANBUS_ERROR; } break; } case Chassis::AUTO_SPEED_ONLY: { if (EnableSpeedOnlyMode() != ErrorCode::OK) { AERROR << "Failed to enable speed only mode"; return ErrorCode::CANBUS_ERROR; } break; } default: break; } return ErrorCode::OK; } ErrorCode VehicleController::Update(const ControlCommand &control_command) { if (!is_initialized_) { AERROR << "Controller is not initialized."; return ErrorCode::CANBUS_ERROR; } // Execute action to transform driving mode if (control_command.has_pad_msg() && control_command.pad_msg().has_action()) { AINFO << "Canbus received pad msg: " << control_command.pad_msg().ShortDebugString(); Chassis::DrivingMode mode = Chassis::COMPLETE_MANUAL; switch (control_command.pad_msg().action()) { case control::DrivingAction::START: { mode = Chassis::COMPLETE_AUTO_DRIVE; break; } case control::DrivingAction::STOP: case control::DrivingAction::RESET: { // In COMPLETE_MANUAL mode break; } default: { AERROR << "No response for this action."; break; } } SetDrivingMode(mode); } if (driving_mode_ == Chassis::COMPLETE_AUTO_DRIVE || driving_mode_ == Chassis::AUTO_SPEED_ONLY) { Gear(control_command.gear_location()); Throttle(control_command.throttle()); Acceleration(control_command.acceleration()); Brake(control_command.brake()); SetEpbBreak(control_command); SetLimits(); } if (driving_mode_ == Chassis::COMPLETE_AUTO_DRIVE || driving_mode_ == Chassis::AUTO_STEER_ONLY) { const double steering_rate_threshold = 1.0; if (control_command.steering_rate() > steering_rate_threshold) { Steer(control_command.steering_target(), control_command.steering_rate()); } else { Steer(control_command.steering_target()); } } if ((driving_mode_ == Chassis::COMPLETE_AUTO_DRIVE || driving_mode_ == Chassis::AUTO_SPEED_ONLY || driving_mode_ == Chassis::AUTO_STEER_ONLY) && control_command.has_signal()) { SetHorn(control_command); SetTurningSignal(control_command); SetBeam(control_command); } return ErrorCode::OK; } } // namespace canbus } // namespace apollo
vehicle_controller 中将各个功能函数都是虚函数,各个函数的功能自行阅读便可,这里主要说一下架构方面的问题。
能够从.cc文件中看出,这里只有 SetDrivingMode() 和 Update() 函数有具体的函数操做内容,其余的虚函数则是在每一个车型文件夹下的 $vehicle_vehicle_controller 会继承 vehicle_controller 基类, 其中有具体操做。
Update() 函数会将具体的收到的控制指令经过对应的函数将指令转译成can报文下发下去,SetDrivingMode() 会根据需求将车辆设置成须要的车辆控制模式。
这里以 Lincoln 为例,具体文件夹下的文件以下图:
能够看出,其中主要有四个主要部分,lincoln_message_manager, lincoln_vehicle_controller, lincoln_vehicle_factory以及protocol文件夹。
这里就不上大量的代码了,只按其中重要的函数进行说明:
canbus模块总体结构比较复杂,仍是须要本身多读两遍的,这里面使用了大量的继承,以及指针,挺复杂的,这里简单的绘制一下具体的模块结构,以及消息传递的流程。
canbus模块结构框架
消息传递流程: