赞
踩
主要输入输出
输入:apollo::control::ControlCommand | 控制指令 输出: /apollo/chassis
| apollo::canbus::Chassis
| 车辆底盘信息接口数据,包括车辆速度、方向盘转角、档位、底盘状态等信息 |
| /apollo/chassis_detail
| apollo::${Vehicle_Type}
| 车辆底盘详细信息,展示发送和接收底盘报文解析数据 |
其中canbus文件夹是canbus模块的主程序入口,构造函数为周期触发的函数,周期执行。
class CanbusComponent final : public apollo::cyber::TimerComponent {
构造函数后进入init函数
bool CanbusComponent::Init() {
AINFO << "CanbusComponent init: ";
// 加载配置文件
if (!GetProtoConfig(&canbus_conf_)) {
AERROR << "Unable to load canbus conf file: " << ConfigFilePath();
return false;
}
AINFO << "The canbus conf file is loaded: " << FLAGS_canbus_conf_file;
ADEBUG << "Canbus_conf:" << canbus_conf_.ShortDebugString();
//加载车辆类型
if (!apollo::cyber::common::PathExists(FLAGS_load_vehicle_library)) {
AERROR << FLAGS_load_vehicle_library << " No such vehicle library";
return false;
}
AINFO << "Load the vehicle factory library: " << FLAGS_load_vehicle_library;
ClassLoader loader(FLAGS_load_vehicle_library);
auto vehicle_object = loader.CreateClassObj<AbstractVehicleFactory>(FLAGS_load_vehicle_class_name);
if (!vehicle_object) {
AERROR << "Failed to create the vehicle factory: " << FLAGS_load_vehicle_class_name;
return false;
}
AINFO << "Successfully create vehicle factory: " << FLAGS_load_vehicle_class_name;
vehicle_object_ = vehicle_object;
if (!vehicle_object_->Init(&canbus_conf_)) {
AERROR << "Fail to init vehicle factory.";
return false;
}
AINFO << "Vehicle factory is successfully initialized.";
// cyber::ReaderConfig guardian_cmd_reader_config;
// guardian_cmd_reader_config.channel_name = FLAGS_guardian_topic;
// guardian_cmd_reader_config.pending_queue_size = FLAGS_guardian_cmd_pending_queue_size;
cyber::ReaderConfig control_cmd_reader_config;
control_cmd_reader_config.channel_name = FLAGS_control_command_topic;
control_cmd_reader_config.pending_queue_size = FLAGS_control_cmd_pending_queue_size;
cyber::ReaderConfig chassis_cmd_reader_config;
chassis_cmd_reader_config.channel_name = FLAGS_chassis_command_topic;
chassis_cmd_reader_config.pending_queue_size = FLAGS_control_cmd_pending_queue_size;
// if (FLAGS_receive_guardian) {
// guardian_cmd_reader_ = node_->CreateReader<GuardianCommand>(
// guardian_cmd_reader_config, [this](const std::shared_ptr<GuardianCommand> &cmd) {
// ADEBUG << "Received guardian data: run canbus callback.";
// OnGuardianCommand(*cmd);
// });
// } else
{
//订阅topic
control_command_reader_ = node_->CreateReader<ControlCommand>(
control_cmd_reader_config, [this](const std::shared_ptr<ControlCommand> &cmd) {
ADEBUG << "Received control data: run canbus callback.";
OnControlCommand(*cmd);
});
chassis_command_reader_ = node_->CreateReader<ChassisCommand>(
chassis_cmd_reader_config, [this](const std::shared_ptr<ChassisCommand> &cmd) {
ADEBUG << "Received control data: run canbus callback.";
OnChassisCommand(*cmd);
});
}
// 发布topic
chassis_writer_ = node_->CreateWriter<Chassis>(FLAGS_chassis_topic);
if (!vehicle_object_->Start()) {
AERROR << "Fail to start canclient, cansender, canreceiver, canclient, "
"vehicle controller.";
Clear();
return false;
}
AINFO << "Start canclient cansender, canreceiver, canclient, vehicle "
"controller successfully.";
monitor_logger_buffer_.INFO("Canbus is started.");
return true;
}
收到控制指令control_command后调用回调函数,将控制指令中的油门、刹车、档位、灯光等指令,转换成对应的报文下发至底盘。vehicle_object_为实例化车辆类型,UpdateCommand跳转到车型内部的更新命令函数。
void CanbusComponent::OnControlCommand(const ControlCommand &control_command) {
int64_t current_timestamp = Time::Now().ToMicrosecond();
// if command coming too soon, just ignore it.
if (current_timestamp - last_timestamp_ < FLAGS_min_cmd_interval * 1000) {
ADEBUG << "Control command comes too soon. Ignore.\n Required "
"FLAGS_min_cmd_interval["
<< FLAGS_min_cmd_interval << "], actual time interval[" << current_timestamp - last_timestamp_ << "].";
return;
}
last_timestamp_ = current_timestamp;
ADEBUG << "Control_sequence_number:" << control_command.header().sequence_num() << ", Time_of_delay:"
<< current_timestamp - static_cast<int64_t>(control_command.header().timestamp_sec() * 1e6)
<< " micro seconds";
vehicle_object_->UpdateCommand(&control_command);
}
回调函数跳转至所选车型的内部更新控制指令。vehicle_controller_->Update为更新控制指令, can_sender_.Update()为can卡将can报文数据发送出去。
void GemVehicleFactory::UpdateCommand(
const apollo::control::ControlCommand *control_command) {
if (vehicle_controller_->Update(*control_command) != ErrorCode::OK) {
AERROR << "Failed to process callback function OnControlCommand because "
"vehicle_controller_->Update error.";
return;
}
can_sender_.Update();
}
跳转的Update函数,在此将控制指令解析,更新控制指令,赋值自动驾驶模式,并对对加速度、减速度、档位等进行赋值。
ErrorCode VehicleController<SensorType>::Update(const ControlCommand &control_command) {
if (!is_initialized_) {
AERROR << "Controller is not initialized.";
return ErrorCode::CANBUS_ERROR;
}
//首先判断初始化
//再检测是否有pad或teleop脚本测试信息传入
//并根据传入的指令进行SetDrivingMode的设置。
// Execute action to transform driving mode
AINFO << " control_command.has_pad_msg() " << control_command.has_pad_msg();
AINFO << " control_command.pad_msg().has_action() " << control_command.pad_msg().has_action();
if (control_command.has_pad_msg() && control_command.pad_msg().has_action()) {
AINFO << "Canbus received pad msg: " << control_command.pad_msg().ShortDebugString();
if (control_command.pad_msg().action() == control::DrivingAction::VIN_REQ) {
if (!VerifyID()) {
AINFO << "Response vid failed, please request again.";
} else {
AINFO << "Response vid success!";
}
} else {
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::RESET: {
break;
}
default: {
AERROR << "No response for this action.";
break;
}
}
auto error_code = SetDrivingMode(mode);//判断DrivingMode的模式,下面展开讲解
if (error_code != ErrorCode::OK) {
AERROR << "Failed to set driving mode.";
} else {
AINFO << "Set driving mode success.";
}
}
}
//再判断自动驾驶状态,并将控制的指令发送至车型的档位、油门、刹车处理函数内
//可根据不同的状态,设置不同的自动驾驶模式
//COMPLETE_AUTO_DRIVE、AUTO_SPEED_ONLY、AUTO_STEER_ONLY
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;
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()) {
HandleVehicleSignal(ProcessCommandChange(control_command.signal(), &last_control_command_));
}
return ErrorCode::OK;
}
SetDrivingMode函数根据传如的DrivingMode的类型进行switch判断,并进行相应的使能
ErrorCode VehicleController<SensorType>::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 (this->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 (this->driving_mode() == driving_mode) {
return ErrorCode::OK;
}
// if current mode is same as previous, no need to set.
// if (this->driving_mode() == driving_mode) {
// AINFO << "this->driving_mode() == driving_mode";
// return ErrorCode::OK;
// }
AINFO << " begin switch driving mode ";
switch (driving_mode) {
case Chassis::COMPLETE_AUTO_DRIVE: {
AINFO << " current is 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;
}
EnableAutoMode函数确定进入automode后,也会跳转至can_sender_->Update();函数进行can报文数据的发送。
ErrorCode GemController::EnableAutoMode() {
if (driving_mode() == Chassis::COMPLETE_AUTO_DRIVE) {
AINFO << "Already in COMPLETE_AUTO_DRIVE mode";
return ErrorCode::OK;
}
global_cmd_69_->set_pacmod_enable(
Global_cmd_69::PACMOD_ENABLE_CONTROL_ENABLED);
global_cmd_69_->set_clear_override(
Global_cmd_69::CLEAR_OVERRIDE_CLEAR_ACTIVE_OVERRIDES);
can_sender_->Update();
const int32_t flag =
CHECK_RESPONSE_STEER_UNIT_FLAG | CHECK_RESPONSE_SPEED_UNIT_FLAG;
if (!CheckResponse(flag, true)) {
AERROR << "Failed to switch to COMPLETE_AUTO_DRIVE mode.";
Emergency();
set_chassis_error_code(Chassis::CHASSIS_ERROR);
return ErrorCode::CANBUS_ERROR;
}
set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
AINFO << "Switch to COMPLETE_AUTO_DRIVE mode ok.";
return ErrorCode::OK;
}
进入车型的档位、油门、刹车处理函数内后:
// NEUTRAL, REVERSE, DRIVE
//根据控制指令内部的档位要求,将档位的枚举值赋值至报文内,从而protocol内会将该值转换为CAN格式的数据
void GemController::Gear(Chassis::GearPosition gear_position) {
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_SPEED_ONLY) {
AINFO << "This drive mode no need to set gear.";
return;
}
switch (gear_position) {
case Chassis::GEAR_NEUTRAL: {
shift_cmd_65_->set_shift_cmd(Shift_cmd_65::SHIFT_CMD_NEUTRAL);
break;
}
case Chassis::GEAR_REVERSE: {
shift_cmd_65_->set_shift_cmd(Shift_cmd_65::SHIFT_CMD_REVERSE);
break;
}
case Chassis::GEAR_DRIVE: {
shift_cmd_65_->set_shift_cmd(Shift_cmd_65::SHIFT_CMD_FORWARD);
break;
}
case Chassis::GEAR_INVALID: {
AERROR << "Gear command is invalid!";
shift_cmd_65_->set_shift_cmd(Shift_cmd_65::SHIFT_CMD_NEUTRAL);
break;
}
default: {
shift_cmd_65_->set_shift_cmd(Shift_cmd_65::SHIFT_CMD_NEUTRAL);
break;
}
}
}
//刹车有不同的类型,有之间使用减速度实际值控制的刹车,也有使用百分比控制的刹车,具体需要根据自己的控制方式和CAN协议进行选择
// brake with new acceleration
// acceleration:0.00~99.99, unit:
// acceleration:0.0 ~ 7.0, unit:m/s^2
// acceleration_spd:60 ~ 100, suggest: 90
// -> pedal
void GemController::Brake(double pedal) {
// double real_value = params_.max_acc() * acceleration / 100;
// TODO(QiL) Update brake value based on mode
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_SPEED_ONLY) {
AINFO << "The current drive mode does not need to set acceleration.";
return;
}
brake_cmd_6b_->set_brake_cmd(pedal / 100.0);
}
// 油门的控制有两种方式,一种是节气门踏板控制,一种是加速度控制,取决于车辆的控制协议
// 节气门踏板控制方式,按照百分比控制
// drive with old acceleration
// gas:0.00~99.99 unit:
void GemController::Throttle(double pedal) {
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_SPEED_ONLY) {
AINFO << "The current drive mode does not need to set acceleration.";
return;
}
accel_cmd_67_->set_accel_cmd(pedal / 100.0);
}
//加速度方式控制,正向加速,负向减速
// drive with acceleration/deceleration
// acc:-7.0 ~ 5.0, unit:m/s^2
void GemController::Acceleration(double acc) {
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_SPEED_ONLY) {
AINFO << "The current drive mode does not need to set acceleration.";
return;
}
// None
}
// 转向角度,可以角度控制,也可以百分比方式控制,但是百分百控制需转换为实际角度,注意方向
// gem default, -470 ~ 470, left:+, right:-
// need to be compatible with control module, so reverse
// steering with old angle speed
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
void GemController::Steer(double angle) {
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_STEER_ONLY) {
AINFO << "The current driving mode does not need to set steer.";
return;
}
const double real_angle = vehicle_params_.max_steer_angle() * angle / 100.0;
steering_cmd_6d_->set_position_value(real_angle)->set_speed_limit(9.0);
}
// 根据转向角和转向角速度,来控制车辆,根据实际的协议进行选择
// steering with new angle speed
// angle:-99.99~0.00~99.99, unit:, left:-, right:+
// angle_spd:0.00~99.99, unit:deg/s
void GemController::Steer(double angle, double angle_spd) {
if (driving_mode() != Chassis::COMPLETE_AUTO_DRIVE &&
driving_mode() != Chassis::AUTO_STEER_ONLY) {
AINFO << "The current driving mode does not need to set steer.";
return;
}
const double real_angle = vehicle_params_.max_steer_angle() * angle / 100.0;
const double real_angle_spd =
ProtocolData<::apollo::canbus::Gem>::BoundedValue(
vehicle_params_.min_steer_angle_rate(),
vehicle_params_.max_steer_angle_rate(),
vehicle_params_.max_steer_angle_rate() * angle_spd / 100.0);
steering_cmd_6d_->set_position_value(real_angle)
->set_speed_limit(real_angle_spd);
}
转向灯光和灯光可以根据具体的协议进行添加
void GemController::SetBeam(const VehicleSignal& vehicle_signal) {
if (vehicle_signal.high_beam()) {
// None
} else if (vehicle_signal.low_beam()) {
// None
} else {
// None
}
}
void GemController::SetHorn(const VehicleSignal& vehicle_signal) {
if (vehicle_signal.horn()) {
// None
} else {
// None
}
}
void GemController::SetTurningSignal(const VehicleSignal& vehicle_signal) {
// Set Turn Signal
auto signal = vehicle_signal.turn_signal();
if (signal == common::VehicleSignal::TURN_LEFT) {
turn_cmd_63_->set_turn_signal_cmd(Turn_cmd_63::TURN_SIGNAL_CMD_LEFT);
} else if (signal == common::VehicleSignal::TURN_RIGHT) {
turn_cmd_63_->set_turn_signal_cmd(Turn_cmd_63::TURN_SIGNAL_CMD_RIGHT);
} else {
turn_cmd_63_->set_turn_signal_cmd(Turn_cmd_63::TURN_SIGNAL_CMD_NONE);
}
}
chassis函数是解析底盘反馈的报文数据,再赋值至chassis消息内,可在cyber_mointor,上监视chassis内部的消息,details内可看报文的具体信息。
Chassis GemController::chassis() {
chassis_.Clear();
Gem chassis_detail;
message_manager_->GetSensorData(&chassis_detail);
// 21, 22, previously 1, 2
if (driving_mode() == Chassis::EMERGENCY_MODE) {
set_chassis_error_code(Chassis::NO_ERROR);
}
chassis_.set_driving_mode(driving_mode());
chassis_.set_error_code(chassis_error_code());
// 3
chassis_.set_engine_started(true);
// 5
if (chassis_detail.has_vehicle_speed_rpt_6f() &&
chassis_detail.vehicle_speed_rpt_6f().has_vehicle_speed()) {
chassis_.set_speed_mps(static_cast<float>(
chassis_detail.vehicle_speed_rpt_6f().vehicle_speed()));
} else {
chassis_.set_speed_mps(0);
}
return chassis_;
}
最后与canbus相关的是drivers内的can驱动,包括了can卡类型,发送的逻辑,可以忽略,后期也可更新上相关的流程。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。