From d1322d1e80309c6d66442f74ba6e302dbdabaa8e Mon Sep 17 00:00:00 2001 From: Heath Henley Date: Mon, 30 Mar 2026 16:30:55 -0400 Subject: [PATCH 1/2] Add raw td proto --- proto/nav_api.proto | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/proto/nav_api.proto b/proto/nav_api.proto index 2d916de..ec91bd5 100644 --- a/proto/nav_api.proto +++ b/proto/nav_api.proto @@ -183,6 +183,47 @@ message TargetData { optional int32 max_range_index = 10; } +// RawTargetData exists for consumers who want to post-process or aggregate raw +// detections on their own. For most use cases, e.g., displaying or using the same +// information shown in SonaSoft, TargetData is a better fit. +// Network details: +// - ZeroMQ pattern: Publish-subscribe +// - Default port: 61505 +message RawTargetData { + // Time stamp for when this data was collected. + optional time.Time time = 1; + // Unique string to identify the sonar this data came from. + optional string serial = 2; + // Heading and lat/lon position at time of data collection. + // Note that the position and heading displayed in the SonaSoft coning display + // may be updating a rate different from the ping rate. Further, this value + // is filtered if Settings -> System Settings -> "Filter heading and position" + // is checked. + optional nav_info.Heading heading = 3; + optional nav_info.Position position = 4; + // Bottom and target bins + repeated Bin bottom = 5; + repeated Bin target = 6; + optional grid_description.GridDescription grid_description = 7; + // Maximum depth of Bins contained in this dataset. + optional double max_depth = 8; + // max_range_index is the maximum number of bins that can occur along a + // horizontal look angle. + optional int32 max_range_index = 9; + // Rolls/tilts + // These are low pass filtered and interpolated to the same range indices + // as the sonar data. They may be empty or have a single value instead under + // error conditions (if there is no data from the sensor). + // Roll measurements are represented as residuals from a common reference. + // `kernel_roll` is the reference roll value, and each entry in `rolls` is + // the residual (roll - kernel_roll) at a given time/sample. + optional float kernel_roll = 10; + repeated float rolls = 11; + repeated float tilts = 12; + // Bin length / range to first bin (needed for mapping back to time) + optional float bin_length = 13; // meters + optional float range_to_first_bin = 14; // meters +} // A Bin is an individual point in the water located relative to the sonar. All // targets (both sea bottom and in-water) are a collection of Bins. Used by the From 80d0b65049f8ae31d27fa41c5b72e34e814b3d9e Mon Sep 17 00:00:00 2001 From: Heath Henley Date: Mon, 30 Mar 2026 16:49:14 -0400 Subject: [PATCH 2/2] Add new message as on option --- include/farsounder/config.hpp | 1 + include/farsounder/subscriber.hpp | 3 + include/farsounder/types.hpp | 17 ++++++ src/config.cpp | 9 ++- src/conversions.cpp | 53 +++++++++++++++++ src/conversions_internal.hpp | 1 + src/subscriber.cpp | 42 +++++++++++++ tests/conversions_unit_tests.cpp | 99 +++++++++++++++++++++++++++++++ 8 files changed, 222 insertions(+), 3 deletions(-) diff --git a/include/farsounder/config.hpp b/include/farsounder/config.hpp index 5407c04..be09fbb 100644 --- a/include/farsounder/config.hpp +++ b/include/farsounder/config.hpp @@ -11,6 +11,7 @@ namespace farsounder::config { enum class PubSubMessage { HydrophoneData, TargetData, + RawTargetData, ProcessorSettings, VesselInfo, }; diff --git a/include/farsounder/subscriber.hpp b/include/farsounder/subscriber.hpp index 9360b9f..f9f33bf 100644 --- a/include/farsounder/subscriber.hpp +++ b/include/farsounder/subscriber.hpp @@ -16,6 +16,7 @@ class Subscriber { public: using HydrophoneCallback = std::function; using TargetCallback = std::function; + using RawTargetCallback = std::function; using ProcessorSettingsCallback = std::function; using VesselInfoCallback = std::function; @@ -31,12 +32,14 @@ class Subscriber { // Register callbacks by message type enum void on(config::PubSubMessage message, HydrophoneCallback callback); void on(config::PubSubMessage message, TargetCallback callback); + void on(config::PubSubMessage message, RawTargetCallback callback); void on(config::PubSubMessage message, ProcessorSettingsCallback callback); void on(config::PubSubMessage message, VesselInfoCallback callback); // Register callbacks by message name string void on(const std::string& message_name, HydrophoneCallback callback); void on(const std::string& message_name, TargetCallback callback); + void on(const std::string& message_name, RawTargetCallback callback); void on(const std::string& message_name, ProcessorSettingsCallback callback); void on(const std::string& message_name, VesselInfoCallback callback); diff --git a/include/farsounder/types.hpp b/include/farsounder/types.hpp index ef28349..23760c8 100644 --- a/include/farsounder/types.hpp +++ b/include/farsounder/types.hpp @@ -155,6 +155,23 @@ struct TargetData { std::int32_t max_range_index{}; }; +struct RawTargetData { + Timestamp time{}; + std::string serial; + std::optional heading; + std::optional position; + std::vector bottom; // Sea floor detections + std::vector target; // In-water detections before grouping + std::optional grid_description; + double max_depth{}; + std::int32_t max_range_index{}; + float kernel_roll{}; + std::vector rolls; + std::vector tilts; + float bin_length{}; + float range_to_first_bin{}; +}; + struct HydrophoneData { Timestamp time{}; std::string serial; diff --git a/src/config.cpp b/src/config.cpp index 6e1d343..bf271c5 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -10,6 +10,7 @@ default_pubsub_ports() { return { {PubSubMessage::HydrophoneData, 61501}, {PubSubMessage::TargetData, 61502}, + {PubSubMessage::RawTargetData, 61505}, {PubSubMessage::ProcessorSettings, 61503}, {PubSubMessage::VesselInfo, 61504}, }; @@ -37,6 +38,7 @@ std::unordered_map pubsub_name_map() { return { {"HydrophoneData", PubSubMessage::HydrophoneData}, {"TargetData", PubSubMessage::TargetData}, + {"RawTargetData", PubSubMessage::RawTargetData}, {"ProcessorSettings", PubSubMessage::ProcessorSettings}, {"VesselInfo", PubSubMessage::VesselInfo}, }; @@ -73,6 +75,8 @@ std::string_view to_string(PubSubMessage message) { return "HydrophoneData"; case PubSubMessage::TargetData: return "TargetData"; + case PubSubMessage::RawTargetData: + return "RawTargetData"; case PubSubMessage::ProcessorSettings: return "ProcessorSettings"; case PubSubMessage::VesselInfo: @@ -184,9 +188,8 @@ ClientConfig build_config( if (subscribe.empty()) { subscribe = { - PubSubMessage::HydrophoneData, - PubSubMessage::TargetData, - PubSubMessage::ProcessorSettings, + PubSubMessage::HydrophoneData, PubSubMessage::TargetData, + PubSubMessage::RawTargetData, PubSubMessage::ProcessorSettings, PubSubMessage::VesselInfo, }; } diff --git a/src/conversions.cpp b/src/conversions.cpp index 86d2f34..03fdc1c 100644 --- a/src/conversions.cpp +++ b/src/conversions.cpp @@ -251,6 +251,59 @@ TargetData convert_target_data(const proto::nav_api::TargetData& t) { return data; } +RawTargetData convert_raw_target_data(const proto::nav_api::RawTargetData& t) { + RawTargetData data; + if (t.has_time()) { + data.time = convert_timestamp(t.time()); + } + data.serial = t.serial(); + + if (t.has_heading()) { + data.heading = Heading{t.heading().heading()}; + } + if (t.has_position()) { + data.position = Position{t.position().lat(), t.position().lon()}; + } + + data.bottom.reserve(static_cast(t.bottom_size())); + for (const auto& bin : t.bottom()) { + data.bottom.push_back(convert_bin(bin)); + } + + data.target.reserve(static_cast(t.target_size())); + for (const auto& bin : t.target()) { + data.target.push_back(convert_bin(bin)); + } + + if (t.has_grid_description()) { + data.grid_description = convert_grid_description(t.grid_description()); + } + data.max_depth = t.max_depth(); + data.max_range_index = t.max_range_index(); + if (t.has_kernel_roll()) { + data.kernel_roll = t.kernel_roll(); + } + + data.rolls.reserve(static_cast(t.rolls_size())); + for (const auto roll : t.rolls()) { + data.rolls.push_back(roll); + } + + data.tilts.reserve(static_cast(t.tilts_size())); + for (const auto tilt : t.tilts()) { + data.tilts.push_back(tilt); + } + + if (t.has_bin_length()) { + data.bin_length = t.bin_length(); + } + if (t.has_range_to_first_bin()) { + data.range_to_first_bin = t.range_to_first_bin(); + } + + return data; +} + ProcessorSettings convert_processor_settings( const proto::nav_api::ProcessorSettings& s) { ProcessorSettings settings; diff --git a/src/conversions_internal.hpp b/src/conversions_internal.hpp index 10507ef..9a5de1a 100644 --- a/src/conversions_internal.hpp +++ b/src/conversions_internal.hpp @@ -25,6 +25,7 @@ GridDescription convert_grid_description( const proto::grid_description::GridDescription& g); HydrophoneData convert_hydrophone_data(const proto::nav_api::HydrophoneData& h); TargetData convert_target_data(const proto::nav_api::TargetData& t); +RawTargetData convert_raw_target_data(const proto::nav_api::RawTargetData& t); ProcessorSettings convert_processor_settings( const proto::nav_api::ProcessorSettings& s); diff --git a/src/subscriber.cpp b/src/subscriber.cpp index cfc05a0..d52eab4 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -98,6 +98,7 @@ struct Subscriber::Impl { std::mutex callback_mutex; std::vector hydrophone_callbacks; std::vector target_callbacks; + std::vector raw_target_callbacks; std::vector processor_settings_callbacks; std::vector vessel_info_callbacks; @@ -123,6 +124,17 @@ struct Subscriber::Impl { } } + void dispatch_message(const RawTargetData& data) { + std::vector callbacks; + { + std::lock_guard lock(callback_mutex); + callbacks = raw_target_callbacks; + } + for (const auto& callback : callbacks) { + callback(data); + } + } + void dispatch_message(const ProcessorSettings& data) { std::vector callbacks; { @@ -216,6 +228,22 @@ struct Subscriber::Impl { } break; } + case config::PubSubMessage::RawTargetData: { + proto::nav_api::RawTargetData proto_data; + if (!proto_data.ParseFromArray( + msg.data(), static_cast(msg.size()))) { + break; + } + auto data = std::make_shared( + detail::convert_raw_target_data(proto_data)); + if (pool) { + pool->enqueue( + [this, data]() { dispatch_message(*data); }); + } else { + dispatch_message(*data); + } + break; + } case config::PubSubMessage::ProcessorSettings: { proto::nav_api::ProcessorSettings proto_data; if (!proto_data.ParseFromArray( @@ -285,6 +313,15 @@ void Subscriber::on(config::PubSubMessage message, TargetCallback callback) { impl_->target_callbacks.push_back(std::move(callback)); } +void Subscriber::on(config::PubSubMessage message, RawTargetCallback callback) { + if (message != config::PubSubMessage::RawTargetData) { + throw std::invalid_argument( + "Callback type does not match pub-sub message"); + } + std::lock_guard lock(impl_->callback_mutex); + impl_->raw_target_callbacks.push_back(std::move(callback)); +} + void Subscriber::on(config::PubSubMessage message, ProcessorSettingsCallback callback) { if (message != config::PubSubMessage::ProcessorSettings) { @@ -314,6 +351,11 @@ void Subscriber::on(const std::string& message_name, TargetCallback callback) { on(config::pubsub_from_name(message_name), std::move(callback)); } +void Subscriber::on(const std::string& message_name, + RawTargetCallback callback) { + on(config::pubsub_from_name(message_name), std::move(callback)); +} + void Subscriber::on(const std::string& message_name, ProcessorSettingsCallback callback) { on(config::pubsub_from_name(message_name), std::move(callback)); diff --git a/tests/conversions_unit_tests.cpp b/tests/conversions_unit_tests.cpp index 693d260..1c82384 100644 --- a/tests/conversions_unit_tests.cpp +++ b/tests/conversions_unit_tests.cpp @@ -19,6 +19,7 @@ using farsounder::detail::convert_fov; using farsounder::detail::convert_fov_to_proto; using farsounder::detail::convert_hydrophone_data; using farsounder::detail::convert_processor_settings; +using farsounder::detail::convert_raw_target_data; using farsounder::detail::convert_result; using farsounder::detail::convert_result_code; using farsounder::detail::convert_system_type; @@ -395,6 +396,104 @@ TEST(TargetDataConversion, HandlesMissingOptionalFields) { EXPECT_EQ(converted.max_range_index, 3); } +TEST(RawTargetDataConversion, CopiesFields) { + proto::nav_api::RawTargetData proto_data; + proto_data.set_serial("serial-raw"); + proto_data.set_max_depth(75.0); + proto_data.set_max_range_index(11); + proto_data.set_kernel_roll(1.25f); + proto_data.add_rolls(0.1f); + proto_data.add_rolls(-0.2f); + proto_data.add_tilts(0.3f); + proto_data.add_tilts(-0.4f); + proto_data.set_bin_length(0.75f); + proto_data.set_range_to_first_bin(8.5f); + auto* time = proto_data.mutable_time(); + time->set_year(1970); + time->set_month(1); + time->set_day(1); + time->set_hour(0); + time->set_minute(0); + time->set_second(2); + time->set_millisecond(0); + auto* heading = proto_data.mutable_heading(); + heading->set_heading(87.5); + auto* position = proto_data.mutable_position(); + position->set_lat(41.0); + position->set_lon(-71.0); + auto* bottom = proto_data.add_bottom(); + bottom->set_hor_index(1); + bottom->set_ver_index(2); + bottom->set_range_index(3); + bottom->set_cross_range(4.0f); + bottom->set_down_range(5.0f); + bottom->set_depth(6.0f); + bottom->set_strength(7.0f); + auto* target = proto_data.add_target(); + target->set_hor_index(8); + target->set_ver_index(9); + target->set_range_index(10); + target->set_cross_range(11.0f); + target->set_down_range(12.0f); + target->set_depth(13.0f); + target->set_strength(14.0f); + auto* grid = proto_data.mutable_grid_description(); + grid->set_mode(proto::grid_description::GridDescription::kFixed); + grid->add_hor_angles(3.0); + grid->add_ver_angles(-3.0); + grid->set_max_range(250.0); + + const auto converted = convert_raw_target_data(proto_data); + EXPECT_EQ(converted.serial, "serial-raw"); + ASSERT_TRUE(converted.heading.has_value()); + EXPECT_DOUBLE_EQ(converted.heading->degrees, 87.5); + ASSERT_TRUE(converted.position.has_value()); + EXPECT_DOUBLE_EQ(converted.position->latitude_degrees, 41.0); + EXPECT_DOUBLE_EQ(converted.position->longitude_degrees, -71.0); + ASSERT_EQ(converted.bottom.size(), 1u); + EXPECT_EQ(converted.bottom[0].hor_index, 1); + ASSERT_EQ(converted.target.size(), 1u); + EXPECT_EQ(converted.target[0].hor_index, 8); + ASSERT_TRUE(converted.grid_description.has_value()); + EXPECT_EQ(converted.grid_description->mode, GridMode::kFixed); + ASSERT_EQ(converted.grid_description->hor_angles.size(), 1u); + EXPECT_DOUBLE_EQ(converted.grid_description->hor_angles[0], 3.0); + ASSERT_EQ(converted.grid_description->ver_angles.size(), 1u); + EXPECT_DOUBLE_EQ(converted.grid_description->ver_angles[0], -3.0); + EXPECT_DOUBLE_EQ(converted.grid_description->max_range, 250.0); + EXPECT_DOUBLE_EQ(converted.max_depth, 75.0); + EXPECT_EQ(converted.max_range_index, 11); + EXPECT_FLOAT_EQ(converted.kernel_roll, 1.25f); + ASSERT_EQ(converted.rolls.size(), 2u); + EXPECT_FLOAT_EQ(converted.rolls[0], 0.1f); + EXPECT_FLOAT_EQ(converted.rolls[1], -0.2f); + ASSERT_EQ(converted.tilts.size(), 2u); + EXPECT_FLOAT_EQ(converted.tilts[0], 0.3f); + EXPECT_FLOAT_EQ(converted.tilts[1], -0.4f); + EXPECT_FLOAT_EQ(converted.bin_length, 0.75f); + EXPECT_FLOAT_EQ(converted.range_to_first_bin, 8.5f); +} + +TEST(RawTargetDataConversion, HandlesMissingOptionalFields) { + proto::nav_api::RawTargetData proto_data; + proto_data.set_serial("serial-raw-minimal"); + + const auto converted = convert_raw_target_data(proto_data); + EXPECT_EQ(converted.serial, "serial-raw-minimal"); + EXPECT_FALSE(converted.heading.has_value()); + EXPECT_FALSE(converted.position.has_value()); + EXPECT_TRUE(converted.bottom.empty()); + EXPECT_TRUE(converted.target.empty()); + EXPECT_FALSE(converted.grid_description.has_value()); + EXPECT_DOUBLE_EQ(converted.max_depth, 0.0); + EXPECT_EQ(converted.max_range_index, 0); + EXPECT_FLOAT_EQ(converted.kernel_roll, 0.0f); + EXPECT_TRUE(converted.rolls.empty()); + EXPECT_TRUE(converted.tilts.empty()); + EXPECT_FLOAT_EQ(converted.bin_length, 0.0f); + EXPECT_FLOAT_EQ(converted.range_to_first_bin, 0.0f); +} + TEST(BasicSanity, ConversionHelpersRemainUsable) { // Keep a small broad sanity for grouped helper usage. proto::nav_api::Bin bin;