Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(hesai): per-sensor diagnostic struct definitions #208

Merged
merged 30 commits into from
Nov 8, 2024
Merged

refactor(hesai): per-sensor diagnostic struct definitions #208

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
30 commits
Select commit Hold shift + click to select a range
53a1080
fix hesaiconfig ptp
ike-kazu Sep 4, 2024
a621baa
chagnge struct name
ike-kazu Sep 4, 2024
b749342
refactor lidaStatus
ike-kazu Sep 30, 2024
404e8bc
fix lidarstatus tcp
ike-kazu Oct 4, 2024
fa87f94
fix lidarmonitor and some issues
ike-kazu Oct 7, 2024
500f751
fix lidarstatus binary issue
ike-kazu Oct 7, 2024
5e4b1c3
refactor lidarConfig
ike-kazu Oct 7, 2024
5dae1b2
fix any codes
ike-kazu Oct 25, 2024
c6a05d4
fix as review
ike-kazu Oct 25, 2024
004d184
fix reviewd points
ike-kazu Oct 28, 2024
aef3445
fix reviewd point
ike-kazu Oct 28, 2024
3553959
Merge branch 'main' into pr/ike-kazu/208
mojomex Oct 29, 2024
1c1ef45
chore(hesai): reduce log spam
mojomex Oct 29, 2024
9f03782
chore(hesai_cmd_response): make all inheritances public
mojomex Oct 29, 2024
4fb5ff9
fix(hesai): correct sensor get command return logic after merge
mojomex Oct 29, 2024
467c3d0
fix(hesai): add missing diagnostics summary, display strings without …
mojomex Oct 29, 2024
c67ca2c
fix(hesai): fix diagnostics keys being output in the wrong categories
mojomex Oct 29, 2024
9ba0284
feat(hesai): change PTP status to ERROR when not synchronized
mojomex Oct 29, 2024
192d3d6
chore(hesai): remove duplicate config printing
mojomex Oct 29, 2024
cb93cb3
chore(hesai): remove unnecessary print statements
mojomex Oct 29, 2024
5cbbe6b
chore(hesai): lessen log spam, error messages
mojomex Oct 29, 2024
5970b99
fix(hesai): add back support for pandar64
mojomex Oct 30, 2024
7ac77d3
fix(hesai): add back support for QT128
mojomex Oct 30, 2024
c64a9c9
fix(hesai): disable voltage monitor output for sensors that don't sup…
mojomex Oct 30, 2024
a3a50a5
chore(hesai_cmd_response): reduce compiler warnings
mojomex Oct 30, 2024
af70dda
fix(hesai_hw_interface): for sensors we couldn't test, fall back to u…
mojomex Oct 30, 2024
ff84899
chore(hesai_hw_interface): remove temporary cxxabi usage
mojomex Oct 30, 2024
0f7c16e
Merge branch 'main' into feat/fix_hesai_all_ptp
mojomex Nov 5, 2024
b98b2d6
fix(nebula_common): properly add nlohmann_json dependency
mojomex Nov 5, 2024
83a226a
fix(pandar64): add calibration_file to schema
mojomex Nov 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
106 changes: 58 additions & 48 deletions ...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <array>
#include <cstdint>
#include <ostream>
#include <queue>
#include <sstream>
#include <string>
#include <nlohmann/json.hpp>
Expand Down Expand Up @@ -280,27 +281,38 @@ struct HesaiConfigBase
json to_json()
{
json j;
std::stringstream ss;
ss << static_cast<int>(get().ipaddr[0]) << "."
<< static_cast<int>(get().ipaddr[1]) << "."
<< static_cast<int>(get().ipaddr[2]) << "."
<< static_cast<int>(get().ipaddr[3]);
j["ipaddr"] = ss.str();
ss << static_cast<int>(get().mask[0]) << "."
<< static_cast<int>(get().mask[1]) << "."
<< static_cast<int>(get().mask[2]) << "."
<< static_cast<int>(get().mask[3]);
j["mask"] = ss.str();
ss << static_cast<int>(get().gateway[0]) << "."
<< static_cast<int>(get().gateway[1]) << "."
<< static_cast<int>(get().gateway[2]) << "."
<< static_cast<int>(get().gateway[3]);
j["gateway"] = ss.str();
ss << static_cast<int>(get().dest_ipaddr[0]) << "."
<< static_cast<int>(get().dest_ipaddr[1]) << "."
<< static_cast<int>(get().dest_ipaddr[2]) << "."
<< static_cast<int>(get().dest_ipaddr[3]);
j["dest_ipaddr"] = ss.str();
{
std::stringstream ss;
ss << static_cast<int>(get().ipaddr[0]) << "."
<< static_cast<int>(get().ipaddr[1]) << "."
<< static_cast<int>(get().ipaddr[2]) << "."
<< static_cast<int>(get().ipaddr[3]);
j["ipaddr"] = ss.str();
}
{
std::stringstream ss;
ss << static_cast<int>(get().mask[0]) << "."
<< static_cast<int>(get().mask[1]) << "."
<< static_cast<int>(get().mask[2]) << "."
<< static_cast<int>(get().mask[3]);
j["mask"] = ss.str();
}
{
std::stringstream ss;
ss << static_cast<int>(get().gateway[0]) << "."
<< static_cast<int>(get().gateway[1]) << "."
<< static_cast<int>(get().gateway[2]) << "."
<< static_cast<int>(get().gateway[3]);
j["gateway"] = ss.str();
}
{
std::stringstream ss;
ss << static_cast<int>(get().dest_ipaddr[0]) << "."
<< static_cast<int>(get().dest_ipaddr[1]) << "."
<< static_cast<int>(get().dest_ipaddr[2]) << "."
<< static_cast<int>(get().dest_ipaddr[3]);
j["dest_ipaddr"] = ss.str();
}
j["dest_LiDAR_udp_port"] = get().dest_LiDAR_udp_port.value();
j["dest_gps_udp_port"] = get().dest_gps_udp_port.value();
j["spin_rate"] = std::to_string(get().spin_rate.value()) + " RPM";
Expand All @@ -321,8 +333,10 @@ struct HesaiConfigBase
return j;
}

virtual json sensor_specifics_to_json() = 0;
[[nodiscard]] virtual const Internal & get() const = 0;

protected:
virtual json sensor_specifics_to_json() = 0;
};

inline std::ostream & operator<<(std::ostream & os, HesaiConfigBase & arg)
Expand Down Expand Up @@ -356,23 +370,14 @@ struct HesaiConfig_OT128_AT128: public HesaiConfigBase

return j;
}
Internal value;
private:
Internal value;
};

inline std::ostream & operator<<(std::ostream & os, HesaiConfig_OT128_AT128 const & arg)
inline std::ostream & operator<<(std::ostream & os, HesaiConfig_OT128_AT128 & arg)
{
os << "gps_nmea_sentence: " << +arg.value.gps_nmea_sentence;
os << ", ";
os << "noise_filtering: " << +arg.value.noise_filtering;
os << ", ";
os << "reflectivity_mapping: " << +arg.value.reflectivity_mapping;
os << ", ";
os << "reserved: ";
for (size_t i = 0; i < sizeof(arg.value.reserved); i++) {
if (i != 0) {
os << ' ';
}
os << std::hex << std::setfill('0') << std::setw(2) << (+arg.value.reserved[i]);
for (auto & [key, value] : arg.to_json().items()) {
os << key << " : " << value << std::endl;
}
return os;
}
Expand Down Expand Up @@ -400,7 +405,8 @@ struct HesaiConfig_XT_40p: public HesaiConfigBase

return j;
}
Internal value;
private:
Internal value;
};

inline std::ostream & operator<<(std::ostream & os, HesaiConfig_XT_40p & arg)
Expand Down Expand Up @@ -429,10 +435,11 @@ struct HesaiLidarStatusBase
return j;
}

// protected:
virtual json sensor_specifics_to_json() = 0;
[[nodiscard]] virtual const Internal & get() const = 0;

protected:
virtual json sensor_specifics_to_json() = 0;

std::string get_str_gps_pps_lock(uint8_t value)
{
switch (value) {
Expand Down Expand Up @@ -489,7 +496,7 @@ struct HesaiLidarStatusAT128: public HesaiLidarStatusBase
big_int32_buf_t startup_times;
big_int32_buf_t total_operation_time;
uint8_t ptp_status;
unsigned char reserve[1];
unsigned char reserved[1];
};
HesaiLidarStatusAT128(Internal value) : value(value) {}
json sensor_specifics_to_json() override
Expand All @@ -511,15 +518,16 @@ struct HesaiLidarStatusAT128: public HesaiLidarStatusBase
j["startup_times"] = value.startup_times.value();
j["total_operation_time"] = std::to_string(value.total_operation_time.value()) + " min";
j["ptp_status"] = get_str_ptp_clock_status(value.ptp_status);
j["reserve"] = value.reserve;
j["reserved"] = value.reserved;

return j;
}
[[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override
{
return value;
}
Internal value;
private:
Internal value;
};

inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatusAT128 & arg)
Expand All @@ -540,7 +548,7 @@ struct HesaiLidarStatusOT128: public HesaiLidarStatusBase
big_int32_buf_t total_operation_time;
uint8_t ptp_status;
big_uint32_buf_t humidity;
unsigned char reserve[1];
unsigned char reserved[1];
};
HesaiLidarStatusOT128(Internal value) : value(value) {}
[[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override
Expand All @@ -566,11 +574,12 @@ struct HesaiLidarStatusOT128: public HesaiLidarStatusBase
j["total_operation_time"] = std::to_string(value.total_operation_time.value()) + " min";
j["ptp_status"] = get_str_ptp_clock_status(value.ptp_status);
j["humidity"] = std::to_string(value.humidity.value() * 0.1) + " %";
j["reserve"] = value.reserve;
j["reserved"] = value.reserved;

return j;
}
Internal value;
private:
Internal value;
};

inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatusOT128 & arg)
Expand All @@ -590,7 +599,7 @@ struct HesaiLidarStatus_XT_40p: public HesaiLidarStatusBase
big_int32_buf_t startup_times;
big_int32_buf_t total_operation_time;
uint8_t ptp_status;
unsigned char reserve[5];
unsigned char reserved[5];
};
HesaiLidarStatus_XT_40p(Internal value) : value(value) {}
[[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override
Expand All @@ -615,11 +624,12 @@ struct HesaiLidarStatus_XT_40p: public HesaiLidarStatusBase
j["startup_times"] = value.startup_times.value();
j["total_operation_time"] = std::to_string(value.total_operation_time.value()) + " min";
j["ptp_status"] = get_str_ptp_clock_status(value.ptp_status);
j["reserve"] = value.reserve;
j["reserved"] = value.reserved;

return j;
}
Internal value;
private:
Internal value;
};

inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatus_XT_40p & arg)
Expand Down
18 changes: 6 additions & 12 deletions nebula_ros/src/hesai/hw_monitor_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,9 +227,8 @@ void HesaiHwMonitorWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatu
std::scoped_lock lock(mtx_lidar_status_);
if (current_status_) {
json data = current_status_->to_json();
for (const auto & [key, value] : data.items()) {
diagnostics.add(key, value);
std::cout << key << " : " << value << std::endl;
if(data.contains("motor_speed")) {
mojomex marked this conversation as resolved.
Show resolved Hide resolved
diagnostics.add("motor_speed", data["motor_speed"]);
}
} else {
diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available");
Expand All @@ -245,7 +244,6 @@ void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWr
json data = current_status_->to_json();
for (const auto & [key, value] : data.items()) {
diagnostics.add(key, value);

}
diagnostics.summary(level, boost::algorithm::join(msg, ", "));
}else {
Expand All @@ -260,10 +258,8 @@ void HesaiHwMonitorWrapper::HesaiCheckTemperature(diagnostic_updater::Diagnostic
std::scoped_lock lock(mtx_lidar_status_);
if (current_status_) {
json data = current_status_->to_json();
for (const auto & [key, value] : data.items()) {
// Skip execpt temperature
if (key != "temperature") {continue;}
for (const auto & [key, value] : value.items()) {
if (data.contains("temperature")) {
for (const auto & [key, value] : data["temperature"].items()) {
diagnostics.add(key, value);
}
}
Expand All @@ -280,10 +276,8 @@ void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWr
uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK;
std::vector<std::string> msg;
json data = current_status_->to_json();
for (const auto & [key, value] : data.items()) {
if(key == "motor_speed") {
diagnostics.add(key, value);
}
if(data.contains("motor_speed")) {
diagnostics.add("motor_speed", data["motor_speed"]);
}
diagnostics.summary(level, boost::algorithm::join(msg, ", "));
} else {
Expand Down
Loading