Skip to content

Commit

Permalink
chore(sync_diag_client): add better error handling, more graph update…
Browse files Browse the repository at this point in the history 
… types

Signed-off-by: Max SCHMELLER <[email protected]>
  • Loading branch information
@mojomex
mojomex committed Feb 26, 2025
1 parent c877866 commit 11d2067
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 30 deletions.
81 changes: 54 additions & 27 deletions nebula_ros/include/nebula_ros/common/sync_diag_client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,9 @@
#include <sync_tooling_msgs/clock_diff_measurement.pb.h>
#include <sync_tooling_msgs/clock_id.pb.h>
#include <sync_tooling_msgs/clock_master_update.pb.h>
#include <sync_tooling_msgs/graph_update.pb.h>
#include <sync_tooling_msgs/port_state.pb.h>
#include <sync_tooling_msgs/port_state_update.pb.h>
#include <sync_tooling_msgs/ptp_parent_update.pb.h>
#include <sync_tooling_msgs/system_clock_id.pb.h>
#include <unistd.h>
Expand All @@ -42,49 +45,72 @@ namespace nebula::ros
class SyncDiagClient
{
public:
SyncDiagClient(std::string master_ip, uint16_t master_port, std::string sensor_id)
using send_result_t = nebula::util::expected<std::monostate, std::string>;

SyncDiagClient(
std::string master_ip, uint16_t master_port, std::string sensor_id, uint8_t ptp_domain_id)
: http_client_(std::move(master_ip), std::to_string(master_port)),
hostname_(get_hostname()),
sensor_id_(std::move(sensor_id))
sensor_id_(std::move(sensor_id)),
ptp_domain_id_(ptp_domain_id)
{
}

void submit_clock_alias(const std::string & ptp_clock_id)
[[nodiscard]] send_result_t submit_clock_alias(const std::string & ptp_clock_id)
{
ClockAliasUpdate u;
u.add_aliases()->mutable_frame_id()->set_frame(sensor_id_);
u.add_aliases()->mutable_ptp_clock_id()->set_id(ptp_clock_id);
send_proto(u);
GraphUpdate gu;
ClockAliasUpdate * u = gu.mutable_clock_alias_update();
u->add_aliases()->mutable_frame_id()->set_frame(sensor_id_);
u->add_aliases()->mutable_ptp_clock_id()->set_id(ptp_clock_id);
return send_proto(gu);
}

void submit_clock_diff_measurement(int64_t diff_ns)
[[nodiscard]] send_result_t submit_clock_diff_measurement(int64_t diff_ns)
{
ClockDiffMeasurement m{};
m.set_diff_ns(diff_ns);
m.mutable_src()->mutable_system_clock_id()->set_hostname(hostname_);
m.mutable_dst()->mutable_frame_id()->set_frame(sensor_id_);
send_proto(m);
GraphUpdate gu;
ClockDiffMeasurement * m = gu.mutable_clock_diff_measurement();
m->set_diff_ns(diff_ns);
m->mutable_src()->mutable_system_clock_id()->set_hostname(hostname_);
m->mutable_dst()->mutable_frame_id()->set_frame(sensor_id_);
return send_proto(gu);
}

void submit_master_update(std::optional<std::string> master_clock_id)
[[nodiscard]] send_result_t submit_master_update(std::optional<std::string> master_clock_id)
{
ClockMasterUpdate u;
u.mutable_clock_id()->mutable_frame_id()->set_frame(sensor_id_);
GraphUpdate gu;
ClockMasterUpdate * u = gu.mutable_clock_master_update();
u->mutable_clock_id()->mutable_frame_id()->set_frame(sensor_id_);
if (master_clock_id) {
u.mutable_master()->mutable_ptp_clock_id()->set_id(master_clock_id.value());
u->mutable_master()->mutable_ptp_clock_id()->set_id(master_clock_id.value());
} else {
u.clear_master();
u->clear_master();
}
send_proto(u);
return send_proto(gu);
}

[[nodiscard]] send_result_t submit_parent_port(
const std::string & parent_clock_id, uint16_t port_number)
{
GraphUpdate gu;
PtpParentUpdate * u = gu.mutable_ptp_parent_update();
u->mutable_clock_id()->mutable_frame_id()->set_frame(sensor_id_);
u->mutable_parent()->mutable_clock_id()->mutable_ptp_clock_id()->set_id(parent_clock_id);
u->mutable_parent()->set_port_number(port_number);
u->mutable_parent()->set_ptp_domain(ptp_domain_id_);
return send_proto(gu);
}

void submit_parent_port(const std::string & parent_clock_id, uint16_t port_number)
[[nodiscard]] send_result_t submit_port_state_update(
const std::string & ptp_clock_id, uint16_t port_number, uint8_t port_state)
{
PtpParentUpdate u;
u.mutable_clock_id()->mutable_frame_id()->set_frame(sensor_id_);
u.mutable_parent()->mutable_clock_id()->mutable_ptp_clock_id()->set_id(parent_clock_id);
u.mutable_parent()->set_port_number(port_number);
send_proto(u);
GraphUpdate gu;
PortStateUpdate * u = gu.mutable_port_state_update();
u->mutable_port_id()->mutable_clock_id()->mutable_ptp_clock_id()->set_id(ptp_clock_id);
u->mutable_port_id()->set_port_number(port_number);
u->mutable_port_id()->set_ptp_domain(ptp_domain_id_);

u->set_port_state(static_cast<PortState>(port_state));
return send_proto(gu);
}

private:
Expand All @@ -101,8 +127,7 @@ class SyncDiagClient
return std::string{hostname_raw.data()};
}

nebula::util::expected<std::monostate, std::string> send_proto(
const google::protobuf::Message & msg)
[[nodiscard]] send_result_t send_proto(const GraphUpdate & msg)
{
bool success = msg.SerializeToString(&serialization_buffer_);
if (!success) {
Expand All @@ -113,6 +138,7 @@ class SyncDiagClient
if (
last_request_ &&
last_request_->wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
last_request_.reset();
return std::string("Previous request still in flight");
}

Expand All @@ -128,6 +154,7 @@ class SyncDiagClient

std::string hostname_;
std::string sensor_id_;
uint8_t ptp_domain_id_;

std::string serialization_buffer_;
std::optional<std::future<drivers::connections::HttpClient::HttpResponse>> last_request_;
Expand Down
16 changes: 13 additions & 3 deletions nebula_ros/src/hesai/hw_monitor_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,8 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper(
RCLCPP_INFO_STREAM(logger_, "Hardware ID: " + hardware_id);

if (config->sync_master) {
sync_diag_client_.emplace(config->sync_master->first, config->sync_master->second, hardware_id);
sync_diag_client_.emplace(
config->sync_master->first, config->sync_master->second, hardware_id, config->ptp_domain);
}

initialize_hesai_diagnostics(monitor_enabled);
Expand Down Expand Up @@ -242,18 +243,27 @@ void HesaiHwMonitorWrapper::on_sync_diag_timer()

try {
auto port_ds = hw_interface_->get_ptp_diag_port();
auto result = sync_diag_client_->submit_port_state_update(
port_ds.portIdentity.clock_id.to_json(), port_ds.portIdentity.port_number.value(),
port_ds.portState);
if (!result.has_value()) {
RCLCPP_WARN_STREAM(logger_, "Could not send port state update: " << result.error());
}
} catch (const std::runtime_error & e) {
RCLCPP_ERROR_STREAM(logger_, "Could not get port dataset from sensor: " << e.what());
}

try {
auto time_status_np = hw_interface_->get_ptp_diag_time();
if (time_status_np.gmPresent.value()) {
sync_diag_client_->submit_master_update(
auto result = sync_diag_client_->submit_master_update(
time_status_np.gmIdentity.to_json().template get<std::string>());
if (!result.has_value()) {
RCLCPP_WARN_STREAM(logger_, "Could not send clock master update: " << result.error());
}
}
} catch (const std::runtime_error & e) {
RCLCPP_ERROR_STREAM(logger_, "Could not get port dataset from sensor: " << e.what());
RCLCPP_ERROR_STREAM(logger_, "Could not get time status dataset from sensor: " << e.what());
}
}

Expand Down

0 comments on commit 11d2067

Please sign in to comment.