《从0开始搭建实现apollo9.0》系列三 CANBUS模块解读

二、CANBUS代码

1、canbus模块的软件架构如下:

在这里插入图片描述
主要输入输出
输入: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卡类型,发送的逻辑,可以忽略,后期也可更新上相关的流程。

  • 4
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
Apollo 9.0是一个自动驾驶开发平台,它提供了一套完整的软硬件解决方案,用于开发和部署自动驾驶系统。交叉编译是在一种平台上生成在另一种平台上运行的可执行文件的过程。在Apollo 9.0中,交叉编译通常用于将代码从开发主机编译为目标平台上的可执行文件。 要进行Apollo 9.0的交叉编译,您需要按照以下步骤进行操作: 1. 配置交叉编译环境:首先,您需要安装目标平台的交叉编译工具链。这些工具链包括交叉编译器、链接器和库文件。您可以从目标平台的官方网站或开发者社区获取这些工具链。 2. 设置环境变量:将交叉编译工具链的路径添加到系统的环境变量中,以便在编译过程中能够正确地找到这些工具。 3. 配置构建系统:Apollo 9.0使用Bazel作为构建系统。您需要根据目标平台的要求,配置Bazel的构建规则和选项。这包括指定目标平台的架构、操作系统和其他相关参数。 4. 编译代码:使用Bazel命令行工具执行编译命令,将代码编译为目标平台上的可执行文件。根据您的需求,您可以选择编译整个Apollo 9.0系统,或者只编译特定的模块或组件。 5. 部署和测试:将编译生成的可执行文件部署到目标平台上,并进行测试和验证。确保代码在目标平台上能够正常运行,并满足性能和功能要求。 请注意,具体的交叉编译步骤可能因为您使用的目标平台和开发环境而有所不同。建议您参考Apollo 9.0的官方文档和开发者社区,以获取更详细和准确的交叉编译指南。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值