diff --git a/libwaterlinked/include/libwaterlinked/protocol.hpp b/libwaterlinked/include/libwaterlinked/protocol.hpp index 6a23259..8c2de18 100644 --- a/libwaterlinked/include/libwaterlinked/protocol.hpp +++ b/libwaterlinked/include/libwaterlinked/protocol.hpp @@ -152,6 +152,9 @@ struct Configuration /// true for normal operation where the DVL periodically searches for bottom lock shorter than the existing bottom /// lock, false if periodic cycling is disabled. bool periodic_cycling_enabled; + + /// true to enable hardware triggering of the DVL, false to disable hardware triggering. Only available for Gen2 DVLs. + std::optional hardware_trigger_enabled; }; /// Response sent by the DVL after a command is sent. diff --git a/libwaterlinked/src/protocol.cpp b/libwaterlinked/src/protocol.cpp index 32e3c09..120a0e1 100644 --- a/libwaterlinked/src/protocol.cpp +++ b/libwaterlinked/src/protocol.cpp @@ -41,6 +41,14 @@ auto from_json(const nlohmann::json & j, Configuration & r) -> void j.at("dark_mode_enabled").get_to(r.dark_mode_enabled); j.at("range_mode").get_to(r.range_mode); j.at("periodic_cycling_enabled").get_to(r.periodic_cycling_enabled); + if (j.contains("hardware_trigger_enabled")) { + bool hardware_trigger_enabled = false; + j.at("hardware_trigger_enabled").get_to(hardware_trigger_enabled); + r.hardware_trigger_enabled = hardware_trigger_enabled; + } else { + // Only available for Gen2 DVLs. + r.hardware_trigger_enabled = std::nullopt; + } } auto to_json(nlohmann::json & j, const Configuration & r) -> void @@ -53,6 +61,9 @@ auto to_json(nlohmann::json & j, const Configuration & r) -> void {"range_mode", r.range_mode}, {"periodic_cycling_enabled", r.periodic_cycling_enabled}, }; + if (r.hardware_trigger_enabled.has_value()) { + j["hardware_trigger_enabled"] = r.hardware_trigger_enabled.value(); + } } auto from_json(const nlohmann::json & j, CommandResponse & r) -> void diff --git a/libwaterlinked/test/test_json.cpp b/libwaterlinked/test/test_json.cpp index 4a8071d..3ba6397 100644 --- a/libwaterlinked/test/test_json.cpp +++ b/libwaterlinked/test/test_json.cpp @@ -95,7 +95,8 @@ TEST(JsonParsing, ParseConfiguration) "dark_mode_enabled":false, "mounting_rotation_offset":20.00, "range_mode":"auto", - "periodic_cycling_enabled":true + "periodic_cycling_enabled":true, + "hardware_trigger_enabled":false } )"; @@ -108,6 +109,23 @@ TEST(JsonParsing, ParseConfiguration) EXPECT_FLOAT_EQ(configuration.mounting_rotation_offset, 20.00); EXPECT_EQ(configuration.range_mode, "auto"); EXPECT_TRUE(configuration.periodic_cycling_enabled); + EXPECT_EQ(configuration.hardware_trigger_enabled, std::optional{false}); + + // Gen1 DVLs do not report hardware trigger support. + const std::string json_string_without_hw_trigger = R"( + { + "speed_of_sound":1475.00, + "acoustic_enabled":true, + "dark_mode_enabled":false, + "mounting_rotation_offset":20.00, + "range_mode":"auto", + "periodic_cycling_enabled":true + } + )"; + + const auto obj_without_hw_trigger = nlohmann::json::parse(json_string_without_hw_trigger); + const auto configuration_without_hw_trigger = obj_without_hw_trigger.get(); + EXPECT_FALSE(configuration_without_hw_trigger.hardware_trigger_enabled.has_value()); } TEST(JsonParsing, ParseCommandResponse) @@ -123,7 +141,8 @@ TEST(JsonParsing, ParseCommandResponse) "dark_mode_enabled":false, "mounting_rotation_offset":20.00, "range_mode":"auto", - "periodic_cycling_enabled":true + "periodic_cycling_enabled":true, + "hardware_trigger_enabled":false }, "format":"json_v3.1", "type":"response" diff --git a/waterlinked_dvl_driver/config/dvl.yaml b/waterlinked_dvl_driver/config/dvl.yaml index 7340f1a..9b127ff 100644 --- a/waterlinked_dvl_driver/config/dvl.yaml +++ b/waterlinked_dvl_driver/config/dvl.yaml @@ -40,3 +40,6 @@ waterlinked_dvl_driver_node: # periodically searching for a bottom lock shorter than the existing bottom # lock periodic_cycling_enabled: true + + # Disable hardware trigger by default. This is only available for Gen2 DVLs. + hardware_trigger_enabled: false diff --git a/waterlinked_dvl_driver/src/waterlinked_dvl_driver.cpp b/waterlinked_dvl_driver/src/waterlinked_dvl_driver.cpp index 09b62a4..a9953f2 100644 --- a/waterlinked_dvl_driver/src/waterlinked_dvl_driver.cpp +++ b/waterlinked_dvl_driver/src/waterlinked_dvl_driver.cpp @@ -77,17 +77,47 @@ auto WaterLinkedDvlDriver::on_configure(const rclcpp_lifecycle::State & /*previo RCLCPP_ERROR(get_logger(), "Failed to create WaterLinkedClient. %s", e.what()); return CallbackReturn::ERROR; } + // Get current configuration + CommandResponse get_config_response; + try { + get_config_response = client_->get_configuration().get(); + } + catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to get current DVL configuration: %s", e.what()); + return CallbackReturn::ERROR; + } + + if (!get_config_response.success) { + RCLCPP_ERROR( + get_logger(), + "Failed to get current DVL configuration: %s", + get_config_response.error_message.c_str()); // NOLINT + return CallbackReturn::ERROR; + } + + Configuration config; + try { + config = get_config_response.result.get(); + } + catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to parse current DVL configuration: %s", e.what()); + return CallbackReturn::ERROR; + } + // Use configured ROS parameters as desired startup configuration. + config.speed_of_sound = static_cast(params_.speed_of_sound); + config.mounting_rotation_offset = static_cast(params_.mounting_rotation_offset); + config.range_mode = params_.range_mode; + config.acoustic_enabled = params_.acoustic_enabled; + config.dark_mode_enabled = params_.dark_mode_enabled; + config.periodic_cycling_enabled = params_.periodic_cycling_enabled; + if (config.hardware_trigger_enabled.has_value()) { + config.hardware_trigger_enabled = params_.hardware_trigger_enabled; + } else if (params_.hardware_trigger_enabled) { + RCLCPP_WARN( + get_logger(), + "Ignoring parameter 'hardware_trigger_enabled=true': this DVL does not report hardware trigger support."); + } - // Set the initial DVL configurations - // This lets users set the default DVL configurations from a parameters/launch file - const Configuration config{ - .speed_of_sound = static_cast(params_.speed_of_sound), - .mounting_rotation_offset = static_cast(params_.mounting_rotation_offset), - .acoustic_enabled = params_.acoustic_enabled, - .dark_mode_enabled = params_.dark_mode_enabled, - .range_mode = params_.range_mode, - .periodic_cycling_enabled = params_.periodic_cycling_enabled, - }; std::future f = client_->set_configuration(config); const CommandResponse response = f.get(); diff --git a/waterlinked_dvl_driver/src/waterlinked_dvl_driver_parameters.yaml b/waterlinked_dvl_driver/src/waterlinked_dvl_driver_parameters.yaml index 9a30248..180470b 100644 --- a/waterlinked_dvl_driver/src/waterlinked_dvl_driver_parameters.yaml +++ b/waterlinked_dvl_driver/src/waterlinked_dvl_driver_parameters.yaml @@ -62,3 +62,9 @@ waterlinked_dvl_driver: default_value: true description: "Whether or not periodic cycling is enabled." read_only: true + + hardware_trigger_enabled: + type: bool + default_value: false + description: "Whether or not hardware trigger is enabled." + read_only: true