Skip to content

Commit

Permalink
Merge pull request #15505 from macDure/master_cm
Browse files Browse the repository at this point in the history 
canbus: update recent canbus changes
  • Loading branch information
@WildBeast114514
WildBeast114514 authored Aug 15, 2024
2 parents ba32187 + bbad8a7 commit ae95c40
Show file tree
Hide file tree
Showing 135 changed files with 8,913 additions and 770 deletions.
219 changes: 184 additions & 35 deletions modules/canbus/canbus_component.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,30 +88,47 @@ bool CanbusComponent::Init() {
chassis_cmd_reader_config.pending_queue_size =
FLAGS_control_cmd_pending_queue_size;

// init cmd reader
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.";
const auto start_time = Time::Now().ToMicrosecond();
OnGuardianCommand(*cmd);
const auto end_time = Time::Now().ToMicrosecond();
if ((end_time - start_time) * 1e-6 > FLAGS_guardian_period) {
AWARN << "Guardian callback time: "
<< (end_time - start_time) * 1e-3 << " ms.";
}
});
} else {
control_command_reader_ = node_->CreateReader<ControlCommand>(
control_cmd_reader_config,
[this](const std::shared_ptr<ControlCommand> &cmd) {
ADEBUG << "Received control data: run canbus callback.";
const auto start_time = Time::Now().ToMicrosecond();
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);
const auto end_time = Time::Now().ToMicrosecond();
if ((end_time - start_time) * 1e-6 > FLAGS_control_period) {
AWARN << "Control callback time: " << (end_time - start_time) * 1e-3
<< " ms.";
}
});
}

// init chassis cmd reader
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);
});

// init chassis writer
chassis_writer_ = node_->CreateWriter<Chassis>(FLAGS_chassis_topic);

// start canbus vehicle
if (!vehicle_object_->Start()) {
AERROR << "Fail to start canclient, cansender, canreceiver, canclient, "
"vehicle controller.";
Expand All @@ -138,54 +155,155 @@ void CanbusComponent::PublishChassis() {
ADEBUG << chassis.ShortDebugString();
}

void CanbusComponent::CheckChassisCommunication() {
bool CanbusComponent::Proc() {
const auto start_time = Time::Now().ToMicrosecond();

if (FLAGS_receive_guardian) {
guardian_cmd_reader_->Observe();
const auto &guardian_cmd_msg = guardian_cmd_reader_->GetLatestObserved();
if (guardian_cmd_msg == nullptr) {
AERROR << "guardian cmd msg is not ready!";
} else {
OnGuardianCommandCheck(*guardian_cmd_msg);
}
} else {
control_command_reader_->Observe();
const auto &control_cmd_msg = control_command_reader_->GetLatestObserved();
if (control_cmd_msg == nullptr) {
AERROR << "control cmd msg is not ready!";
} else {
OnControlCommandCheck(*control_cmd_msg);
}
}

// check can receiver msg lost
if (vehicle_object_->CheckChassisCommunicationFault()) {
AERROR << "Can not get the chassis info, please check the chassis "
"communication!";
is_chassis_communicate_lost_ = true;
} else {
is_chassis_communicate_lost_ = false;
}
}

bool CanbusComponent::Proc() {
CheckChassisCommunication();
// publish "/apollo/canbus/chassis"
PublishChassis();
if (!is_chassis_communicate_lost_) {
if (FLAGS_enable_chassis_detail_pub) {
vehicle_object_->PublishChassisDetail();
}

// publish "/apollo/canbus/chassis_detail"
if (FLAGS_enable_chassis_detail_pub) {
vehicle_object_->PublishChassisDetail();
}

// publish "/apollo/canbus/chassis_detail_sender"
if (FLAGS_enable_chassis_detail_sender_pub) {
vehicle_object_->PublishChassisDetailSender();
}

// update heartbeat in can sender
vehicle_object_->UpdateHeartbeat();

const auto end_time = Time::Now().ToMicrosecond();
const double time_diff_ms = (end_time - start_time) * 1e-3;
if (time_diff_ms > (1 / FLAGS_chassis_freq * 1e3)) {
AWARN << "CanbusComponent::Proc() takes too much time: " << time_diff_ms
<< " ms";
}

return true;
}

void CanbusComponent::OnControlCommand(const ControlCommand &control_command) {
int64_t current_timestamp = Time::Now().ToMicrosecond();
// us : microsecord = 1e-3 millisecond = 1e-6 second
double current_timestamp = Time::Now().ToMicrosecond();
// if command coming too soon, just ignore it.
// us < 5 ms(millisecond) *1000 (=5000us microsecord)
if (current_timestamp - last_timestamp_controlcmd_ <
FLAGS_min_cmd_interval * 1000) {
ADEBUG << "Control command comes too soon. Ignore.\n Required "
ADEBUG << "Control command comes too soon. Ignore. Required "
"FLAGS_min_cmd_interval["
<< FLAGS_min_cmd_interval << "], actual time interval["
<< current_timestamp - last_timestamp_controlcmd_ << "].";
<< FLAGS_min_cmd_interval << "] ms, actual time interval["
<< (current_timestamp - last_timestamp_controlcmd_) * 1e-3
<< "] ms.";
return;
}

last_timestamp_controlcmd_ = 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);
if (!is_control_cmd_time_delay_) {
vehicle_object_->UpdateCommand(&control_command);
}
}

void CanbusComponent::OnControlCommandCheck(
const ControlCommand &control_command) {
// us : microsecord = 1e-3 millisecond = 1e-6 second
double current_timestamp = Time::Now().ToMicrosecond();
// cmd_time_diff: s
double cmd_time_diff =
current_timestamp * 1e-6 - control_command.header().timestamp_sec();
if (FLAGS_use_control_cmd_check &&
(cmd_time_diff > (FLAGS_max_control_miss_num * FLAGS_control_period))) {
AERROR << "Control cmd timeout, sequence_number:"
<< control_command.header().sequence_num()
<< ", Time_of_delay:" << cmd_time_diff << " s"
<< ", time delay threshold: "
<< (FLAGS_max_control_miss_num * FLAGS_control_period) << " s";

if (vehicle_object_->Driving_Mode() == Chassis::COMPLETE_AUTO_DRIVE ||
vehicle_object_->Driving_Mode() == Chassis::AUTO_STEER_ONLY ||
vehicle_object_->Driving_Mode() == Chassis::AUTO_SPEED_ONLY) {
is_control_cmd_time_delay_ = true;
GuardianCommand new_guardian_command;
new_guardian_command.mutable_control_command()->CopyFrom(control_command);
ProcessGuardianCmdTimeout(&new_guardian_command);
ADEBUG << "new_guardian_command is "
<< new_guardian_command.ShortDebugString();
vehicle_object_->UpdateCommand(&new_guardian_command.control_command());
}
} else {
is_control_cmd_time_delay_ = false;
}
}

void CanbusComponent::OnGuardianCommand(
const GuardianCommand &guardian_command) {
if (!is_control_cmd_time_delay_) {
OnControlCommand(guardian_command.control_command());
}
}

void CanbusComponent::OnGuardianCommandCheck(
const GuardianCommand &guardian_command) {
// us : microsecord = 1e-3 millisecond = 1e-6 second
double current_timestamp = Time::Now().ToMicrosecond();
// cmd_time_diff: s
double guardian_cmd_time_diff =
current_timestamp * 1e-6 - guardian_command.header().timestamp_sec();
if (FLAGS_use_guardian_cmd_check &&
(guardian_cmd_time_diff >
(FLAGS_max_guardian_miss_num * FLAGS_guardian_period))) {
AERROR << "Guardain cmd timeout, sequence_number:"
<< guardian_command.header().sequence_num()
<< ", Time_of_delay:" << guardian_cmd_time_diff << " s"
<< ", time delay threshold: "
<< (FLAGS_max_guardian_miss_num * FLAGS_guardian_period) << " s";

if (vehicle_object_->Driving_Mode() == Chassis::COMPLETE_AUTO_DRIVE ||
vehicle_object_->Driving_Mode() == Chassis::AUTO_STEER_ONLY ||
vehicle_object_->Driving_Mode() == Chassis::AUTO_SPEED_ONLY) {
is_control_cmd_time_delay_ = true;
GuardianCommand new_guardian_command;
new_guardian_command.CopyFrom(guardian_command);
ProcessGuardianCmdTimeout(&new_guardian_command);
ADEBUG << "new_guardian_command is "
<< new_guardian_command.ShortDebugString();
vehicle_object_->UpdateCommand(&new_guardian_command.control_command());
}
} else {
is_control_cmd_time_delay_ = false;
}
}

void CanbusComponent::OnChassisCommand(const ChassisCommand &chassis_command) {
// us : microsecord = 1e-3 millisecond = 1e-6 second
int64_t current_timestamp = Time::Now().ToMicrosecond();
// if command coming too soon, just ignore it.
// us < 5 ms(millisecond) *1000 (=5000us microsecord)
if (current_timestamp - last_timestamp_chassiscmd_ <
FLAGS_min_cmd_interval * 1000) {
ADEBUG << "Control command comes too soon. Ignore.\n Required "
Expand All @@ -194,8 +312,8 @@ void CanbusComponent::OnChassisCommand(const ChassisCommand &chassis_command) {
<< current_timestamp - last_timestamp_chassiscmd_ << "].";
return;
}

last_timestamp_chassiscmd_ = current_timestamp;

ADEBUG << "Control_sequence_number:"
<< chassis_command.header().sequence_num() << ", Time_of_delay:"
<< current_timestamp -
Expand All @@ -206,15 +324,46 @@ void CanbusComponent::OnChassisCommand(const ChassisCommand &chassis_command) {
vehicle_object_->UpdateCommand(&chassis_command);
}

void CanbusComponent::OnGuardianCommand(
const GuardianCommand &guardian_command) {
OnControlCommand(guardian_command.control_command());
}

common::Status CanbusComponent::OnError(const std::string &error_msg) {
monitor_logger_buffer_.ERROR(error_msg);
return ::apollo::common::Status(ErrorCode::CANBUS_ERROR, error_msg);
}

void CanbusComponent::ProcessTimeoutByClearCanSender() {
if (vehicle_object_->Driving_Mode() != Chassis::COMPLETE_AUTO_DRIVE &&
vehicle_object_->Driving_Mode() != Chassis::AUTO_STEER_ONLY &&
vehicle_object_->Driving_Mode() != Chassis::AUTO_SPEED_ONLY &&
!FLAGS_chassis_debug_mode) {
ADEBUG << "The current driving mode does not need to check cmd timeout.";
if (vehicle_object_->IsSendProtocolClear()) {
AINFO << "send protocol is clear, ignore driving mode, need to recover "
"send protol.";
vehicle_object_->AddSendProtocol();
}
return;
}

if (!is_control_cmd_time_delay_previous_ && is_control_cmd_time_delay_) {
AINFO << "control cmd time latency delay, clear send protocol.";
vehicle_object_->ClearSendProtocol();
} else if (is_control_cmd_time_delay_previous_ &&
!is_control_cmd_time_delay_) {
AINFO << "control cmd time latency reover, add send protocol.";
if (vehicle_object_->IsSendProtocolClear()) {
vehicle_object_->AddSendProtocol();
}
}
is_control_cmd_time_delay_previous_ = is_control_cmd_time_delay_;
}

void CanbusComponent::ProcessGuardianCmdTimeout(
GuardianCommand *guardian_command) {
AINFO << "Into cmd timeout process, set estop.";
guardian_command->mutable_control_command()->set_throttle(0.0);
guardian_command->mutable_control_command()->set_steering_target(0.0);
guardian_command->mutable_control_command()->set_steering_rate(25.0);
guardian_command->mutable_control_command()->set_brake(FLAGS_estop_brake);
}

} // namespace canbus
} // namespace apollo
14 changes: 10 additions & 4 deletions modules/canbus/canbus_component.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,13 +82,18 @@ class CanbusComponent final : public apollo::cyber::TimerComponent {

void PublishChassis();
void OnControlCommand(const apollo::control::ControlCommand &control_command);
void OnControlCommandCheck(
const apollo::control::ControlCommand &control_command);
void OnChassisCommand(
const apollo::external_command::ChassisCommand &chassis_command);
void OnGuardianCommand(
const apollo::guardian::GuardianCommand &guardian_command);
void OnGuardianCommandCheck(
const apollo::guardian::GuardianCommand &guardian_command);
apollo::common::Status OnError(const std::string &error_msg);
void RegisterCanClients();
void CheckChassisCommunication();
void ProcessTimeoutByClearCanSender();
void ProcessGuardianCmdTimeout(
apollo::guardian::GuardianCommand *guardian_command);

CanbusConf canbus_conf_;
std::shared_ptr<::apollo::canbus::AbstractVehicleFactory> vehicle_object_ =
Expand All @@ -99,11 +104,12 @@ class CanbusComponent final : public apollo::cyber::TimerComponent {
control_command_reader_;
std::shared_ptr<cyber::Reader<apollo::external_command::ChassisCommand>>
chassis_command_reader_;
int64_t last_timestamp_controlcmd_ = 0;
double last_timestamp_controlcmd_ = 0.0;
int64_t last_timestamp_chassiscmd_ = 0;
::apollo::common::monitor::MonitorLogBuffer monitor_logger_buffer_;
std::shared_ptr<cyber::Writer<Chassis>> chassis_writer_;
bool is_chassis_communicate_lost_ = false;
bool is_control_cmd_time_delay_ = false;
bool is_control_cmd_time_delay_previous_ = false;
};

CYBER_REGISTER_COMPONENT(CanbusComponent)
Expand Down
22 changes: 20 additions & 2 deletions modules/canbus/common/canbus_gflags.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,24 @@ DEFINE_string(canbus_conf_file,

// Canbus gflags
DEFINE_double(chassis_freq, 100, "Chassis feedback timer frequency.");

// cmd input check
DEFINE_int64(min_cmd_interval, 5, "Minimum control command interval in ms.");
DEFINE_int64(pad_msg_delay_interval, 3,
"Minimum pad msg command delay interval in s.");
DEFINE_int32(max_control_miss_num, 5, "max control miss num.");
DEFINE_double(control_period, 0.01, "control period in s.");
DEFINE_int32(max_guardian_miss_num, 5, "max guardian miss num.");
DEFINE_double(guardian_period, 0.01, "control period in s.");
DEFINE_bool(use_control_cmd_check, false, "enable control cmd input check.");
DEFINE_bool(use_guardian_cmd_check, false, "nable guardian cmd input check.");
DEFINE_double(estop_brake, 30.0, "brake action when cmd input check error.");

// chassis_detail message publish
DEFINE_bool(enable_chassis_detail_pub, false, "Chassis Detail message publish");
DEFINE_bool(enable_chassis_detail_pub, true,
"Chassis Detail receive message publish");
DEFINE_bool(enable_chassis_detail_sender_pub, true,
"Chassis Detail sender message publish");

// canbus test files
DEFINE_string(canbus_test_file,
Expand All @@ -48,8 +62,12 @@ DEFINE_int32(control_cmd_pending_queue_size, 10,
"Max control cmd pending queue size");
DEFINE_int32(chassis_cmd_pending_queue_size, 10,
"Max control cmd pending queue size");

// enable forward Ultrasonic AEB
DEFINE_bool(enable_aeb, true, "Enable forward Ultrasonic AEB");
DEFINE_bool(enable_aeb, false, "Enable forward Ultrasonic AEB");

// enabel chassis debug mode for such as ignore pad msg timestamp check
DEFINE_bool(chassis_debug_mode, false, "Enable chassis in debug mode");

// vehicle factory dynamic library path and class name
DEFINE_string(load_vehicle_library,
Expand Down
Loading

0 comments on commit ae95c40

Please sign in to comment.