From 9b747f94a77b93348a8faf8d169a9b3949136cc7 Mon Sep 17 00:00:00 2001 From: Jean-Luc Bedwani Date: Thu, 28 Nov 2024 09:43:58 -0500 Subject: [PATCH] Correction of marine_acoustics DVL message packing. --- include/dvl_a50/dvl_a50.hpp | 2 +- src/dvl_a50_ros2.cpp | 13 +++++++++---- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/include/dvl_a50/dvl_a50.hpp b/include/dvl_a50/dvl_a50.hpp index a21c302..69aa714 100644 --- a/include/dvl_a50/dvl_a50.hpp +++ b/include/dvl_a50/dvl_a50.hpp @@ -37,7 +37,7 @@ class DvlA50 int speed_of_sound, bool acoustic_enabled, bool led_enabled, - int mountig_rotation_offset, + int mounting_rotation_offset, std::string range_mode); void set_speed_of_sound(int speed_of_sound); diff --git a/src/dvl_a50_ros2.cpp b/src/dvl_a50_ros2.cpp index 89e846b..10f003f 100644 --- a/src/dvl_a50_ros2.cpp +++ b/src/dvl_a50_ros2.cpp @@ -246,14 +246,19 @@ class DvlA50Node : public rclcpp_lifecycle::LifecycleNode velocity_report.beam_ranges_valid = true; velocity_report.beam_velocities_valid = res["velocity_valid"]; + velocity_report.num_good_beams = 0; // Beam specific data for (size_t beam = 0; beam < 4; beam++) { - velocity_report.num_good_beams += bool(res["transducers"][beam]["beam_valid"]); - velocity_report.range = res["transducers"][beam]["distance"]; + if (res["transducers"][beam]["beam_valid"]) + { + velocity_report.num_good_beams++; + } + + velocity_report.range[beam] = double(res["transducers"][beam]["distance"]); //velocity_report.range_covar - velocity_report.beam_quality = res["transducers"][beam]["rssi"]; - velocity_report.beam_velocity = res["transducers"][beam]["velocity"]; + velocity_report.beam_quality[beam] = res["transducers"][beam]["rssi"]; + velocity_report.beam_velocity[beam] = res["transducers"][beam]["velocity"]; //velocity_report.beam_velocity_covar }