Skip to content

Commit

Permalink
fix(hesai_hw_interface): add missing check for PTC error, make error …
Browse files Browse the repository at this point in the history 
…type more readable
  • Loading branch information
@mojomex
mojomex committed Apr 22, 2024
1 parent 02bb644 commit 4d3bf5b
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 53 deletions.
24 changes: 15 additions & 9 deletions ...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,21 +93,28 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117;
const uint16_t MTU_SIZE = 1500;

// Time interval between Announce messages, in units of log seconds (default: 1)
const int PTP_LOG_ANNOUNCE_INTERVAL = 1;
const int PTP_LOG_ANNOUNCE_INTERVAL = 1;
// Time interval between Sync messages, in units of log seconds (default: 1)
const int PTP_SYNC_INTERVAL = 1;
const int PTP_SYNC_INTERVAL = 1;
// Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0)
const int PTP_LOG_MIN_DELAY_INTERVAL = 0;

const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0;
const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1;


/// @brief Hardware interface of hesai driver
class HesaiHwInterface : NebulaHwInterfaceBase
{
private:
typedef nebula::util::expected<std::vector<uint8_t>, uint32_t> ptc_cmd_result_t;
struct ptc_error_t
{
uint8_t error_flags = 0;
uint8_t ptc_error_code = 0;

bool ok() { return !error_flags && !ptc_error_code; }
};

typedef nebula::util::expected<std::vector<uint8_t>, ptc_error_t> ptc_cmd_result_t;

std::unique_ptr<::drivers::common::IoContext> cloud_io_context_;
std::shared_ptr<boost::asio::io_context> m_owned_ctx;
Expand Down Expand Up @@ -160,18 +167,17 @@ class HesaiHwInterface : NebulaHwInterfaceBase
void PrintDebug(const std::vector<uint8_t> & bytes);

/// @brief Convert an error code to a human-readable string
/// @param error_code The error code (lowest byte is Hesai PTC error code, higher bytes nebula flags)
/// such as TCP_ERROR_UNRELATED_RESPONSE etc.
/// @param error_code The error code, containing the sensor's error code (if any), along with
/// flags such as TCP_ERROR_UNRELATED_RESPONSE etc.
/// @return A string description of all errors in this code
std::string PrettyPrintPTCError(uint32_t error_code);
std::string PrettyPrintPTCError(ptc_error_t error_code);

/// @brief Send a PTC request with an optional payload, and return the full response payload.
/// Blocking.
/// @param command_id PTC command number.
/// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header.
/// @return The returned payload, if successful, or nullptr.
ptc_cmd_result_t SendReceive(
const uint8_t command_id, const std::vector<uint8_t> & payload = {});
ptc_cmd_result_t SendReceive(const uint8_t command_id, const std::vector<uint8_t> & payload = {});

public:
/// @brief Constructor
Expand Down
93 changes: 49 additions & 44 deletions nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive(
auto response_complete = std::make_shared<bool>(false);

// Low byte is for PTC error code, the rest is nebula-specific
auto error_code = std::make_shared<uint32_t>(0);
auto error_code = std::make_shared<ptc_error_t>();

std::stringstream ss;
ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast<int>(command_id) << " (" << len << ") ";
Expand All @@ -89,14 +89,15 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive(
tcp_driver_->asyncSendReceiveHeaderPayload(
send_buf,
[this, log_tag, command_id, response_complete, error_code](const std::vector<uint8_t> & header_bytes) {
error_code->ptc_error_code = header_bytes[3];

size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7];
PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)");
// If command_id in the response does not match, we got a response for another command (or rubbish), probably as a
// result of too many simultaneous TCP connections to the sensor (e.g. from GUI, Web UI, another nebula instance, etc.)
if (header_bytes[2] != command_id) {
*error_code = (TCP_ERROR_UNRELATED_RESPONSE << 8);
error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE;
}
*error_code |= header_bytes[3];
if (payload_len == 0) {
*response_complete = true;
}
Expand All @@ -107,7 +108,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive(
// Header had payload length 0 (thus, header callback processed request successfully already),
// but we still received a payload: invalid state
if (*response_complete == true) {
*error_code |= (TCP_ERROR_UNEXPECTED_PAYLOAD << 8);
error_code->error_flags |= TCP_ERROR_UNEXPECTED_PAYLOAD;
return;
}

Expand All @@ -123,13 +124,17 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive(
this->IOContextRun();
if (!tm.try_lock_for(std::chrono::seconds(1))) {
PrintError(log_tag + "Request did not finish within 1s");
*error_code |= (TCP_ERROR_TIMEOUT << 8);
error_code->error_flags |= TCP_ERROR_TIMEOUT;
return *error_code;
}

if (!response_complete) {
PrintError(log_tag + "Did not receive response");
*error_code |= (TCP_ERROR_INCOMPLETE_RESPONSE << 8);
error_code->error_flags |= TCP_ERROR_INCOMPLETE_RESPONSE;
return *error_code;
}

if (!error_code->ok()) {
return *error_code;
}

Expand Down Expand Up @@ -361,21 +366,21 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str)
std::vector<uint8_t> HesaiHwInterface::GetLidarCalibrationBytes()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION);
return response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
return response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
}

std::string HesaiHwInterface::GetLidarCalibrationString()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION);
auto calib_data = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto calib_data = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
std::string calib_string(calib_data.begin(), calib_data.end());
return calib_string;
}

HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus()
{
auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS});
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiPtpDiagStatus)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -394,7 +399,7 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus()
HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort()
{
auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET});
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiPtpDiagPort)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -412,7 +417,7 @@ HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort()
HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime()
{
auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP});
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiPtpDiagTime)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -431,7 +436,7 @@ HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime()
HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster()
{
auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP});
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiPtpDiagGrandmaster)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -450,7 +455,7 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster()
HesaiInventory HesaiHwInterface::GetInventory()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO);
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() < sizeof(HesaiInventory)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -467,7 +472,7 @@ HesaiInventory HesaiHwInterface::GetInventory()
HesaiConfig HesaiHwInterface::GetConfig()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO);
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiConfig)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -483,7 +488,7 @@ HesaiConfig HesaiHwInterface::GetConfig()
HesaiLidarStatus HesaiHwInterface::GetLidarStatus()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS);
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiLidarStatus)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -502,7 +507,7 @@ Status HesaiHwInterface::SetSpinRate(uint16_t rpm)
request_payload.emplace_back(rpm & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -514,7 +519,7 @@ Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle)
request_payload.emplace_back(angle & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -524,7 +529,7 @@ Status HesaiHwInterface::SetTriggerMethod(int trigger_method)
request_payload.emplace_back(trigger_method & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -534,7 +539,7 @@ Status HesaiHwInterface::SetStandbyMode(int standby_mode)
request_payload.emplace_back(standby_mode & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -544,7 +549,7 @@ Status HesaiHwInterface::SetReturnMode(int return_mode)
request_payload.emplace_back(return_mode & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -562,7 +567,7 @@ Status HesaiHwInterface::SetDestinationIp(
request_payload.emplace_back(gps_port & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -588,7 +593,7 @@ Status HesaiHwInterface::SetControlPort(
request_payload.emplace_back(vlan_id & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -602,7 +607,7 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector<unsigned char> da
request_payload.insert(request_payload.end(), data.begin(), data.end());

auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -620,14 +625,14 @@ Status HesaiHwInterface::SetLidarRange(int start, int end)
request_payload.emplace_back(end & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

HesaiLidarRangeAll HesaiHwInterface::GetLidarRange()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE);
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() < 1) {
throw std::runtime_error("Response payload too short");
Expand Down Expand Up @@ -661,7 +666,7 @@ Status HesaiHwInterface::SetClockSource(int clock_source)
request_payload.emplace_back(clock_source & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand Down Expand Up @@ -695,14 +700,14 @@ Status HesaiHwInterface::SetPtpConfig(
}

auto response_or_err = SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

HesaiPtpConfig HesaiHwInterface::GetPtpConfig()
{
auto response_or_err = SendReceive(PTC_COMMAND_GET_PTP_CONFIG);
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() < sizeof(HesaiPtpConfig)) {
throw std::runtime_error("Unexpected payload size");
Expand All @@ -726,7 +731,7 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig()
Status HesaiHwInterface::SendReset()
{
auto response_or_err = SendReceive(PTC_COMMAND_RESET);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

Expand All @@ -736,14 +741,14 @@ Status HesaiHwInterface::SetRotDir(int mode)
request_payload.emplace_back(mode & 0xff);

auto response_or_err = SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload);
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
return Status::OK;
}

HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor()
{
auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR);
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or(0)));
auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));

if (response.size() != sizeof(HesaiLidarMonitor)) {
throw std::runtime_error("Unexpected payload size");
Expand Down Expand Up @@ -1353,20 +1358,20 @@ void HesaiHwInterface::PrintDebug(const std::vector<uint8_t> & bytes)
PrintDebug(ss.str());
}

std::string HesaiHwInterface::PrettyPrintPTCError(uint32_t error_code) {
if (error_code == 0) {
std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) {
if (error_code.ok()) {
return "No error";
}

uint8_t hesai_error = error_code & 0xFF;
uint32_t nebula_error_flags = (error_code >> 8);
auto ptc_error = error_code.ptc_error_code;
auto error_flags = error_code.error_flags;
std::stringstream ss;

if (hesai_error) {
ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast<int>(hesai_error) << ' ';
if (ptc_error) {
ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast<int>(ptc_error) << ' ';
}

switch(hesai_error) {
switch(ptc_error) {
case PTC_ERROR_CODE_NO_ERROR:
break;
case PTC_ERROR_CODE_INVALID_INPUT_PARAM:
Expand Down Expand Up @@ -1395,27 +1400,27 @@ std::string HesaiHwInterface::PrettyPrintPTCError(uint32_t error_code) {
break;
}

if (!nebula_error_flags) {
if (!error_flags) {
return ss.str();
}

if (hesai_error) {
if (ptc_error) {
ss << ", ";
}

ss << "Communication error: ";
std::vector<std::string> nebula_errors;

if (nebula_error_flags & TCP_ERROR_INCOMPLETE_RESPONSE) {
if (error_flags & TCP_ERROR_INCOMPLETE_RESPONSE) {
nebula_errors.push_back("Incomplete response payload");
}
if (nebula_error_flags & TCP_ERROR_TIMEOUT) {
if (error_flags & TCP_ERROR_TIMEOUT) {
nebula_errors.push_back("Request timeout");
}
if (nebula_error_flags & TCP_ERROR_UNEXPECTED_PAYLOAD) {
if (error_flags & TCP_ERROR_UNEXPECTED_PAYLOAD) {
nebula_errors.push_back("Received payload but expected payload length 0");
}
if (nebula_error_flags & TCP_ERROR_UNRELATED_RESPONSE) {
if (error_flags & TCP_ERROR_UNRELATED_RESPONSE) {
nebula_errors.push_back("Received unrelated response");
}

Expand Down

0 comments on commit 4d3bf5b

Please sign in to comment.