diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index 5cbff412..3b377c85 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -340,6 +340,31 @@ tethys + + + + + linear_battery + 14.4 + 14.4 + true + + 400 + 400 + + 0.003064 + + 28.8 + + + + + true + true + 42.1 + + diff --git a/lrauv_ignition_plugins/proto/lrauv_ignition_plugins/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_ignition_plugins/lrauv_state.proto index c9ae1783..bfb967af 100644 --- a/lrauv_ignition_plugins/proto/lrauv_ignition_plugins/lrauv_state.proto +++ b/lrauv_ignition_plugins/proto/lrauv_ignition_plugins/lrauv_state.proto @@ -167,4 +167,16 @@ message LRAUVState // 0: Data collected from Chlorophyll sensor. Unit: ug / L // 1: Pressure calculated from current depth and latitude. Unit: Pa repeated float values_ = 32; + + /// \brief Voltage of the battery. + double batteryVoltage_ = 33; + + /// \brief Current supplied by the battery in A. + double batteryCurrent_ = 34; + + /// \brief Current charge in the battery in Ah. + double batteryCharge_ = 35; + + /// \brief Percentage of battery remaining. + double batteryPercentage_ = 36; } diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index 46112160..67e9c372 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -350,6 +350,15 @@ void TethysCommPlugin::SetupControlTopics(const std::string &_ns) << this->temperatureTopic << "]. " << std::endl; } + this->batteryTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + _ns + "/" + this->batteryTopic); + if (!this->node.Subscribe(this->batteryTopic, + &TethysCommPlugin::BatteryCallback, this)) + { + ignerr << "Error subscribing to topic " << "[" + << this->batteryTopic << "]. " << std::endl; + } + this->chlorophyllTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + _ns + "/" + this->chlorophyllTopic); if (!this->node.Subscribe(this->chlorophyllTopic, @@ -489,6 +498,15 @@ void TethysCommPlugin::TemperatureCallback( this->latestTemperature.SetCelsius(_msg.data()); } +void TethysCommPlugin::BatteryCallback( + const gz::msgs::BatteryState &_msg) +{ + this->latestBatteryVoltage = _msg.voltage(); + this->latestBatteryCurrent = _msg.current(); + this->latestBatteryCharge = _msg.charge(); + this->latestBatteryPercentage = _msg.percentage(); +} + void TethysCommPlugin::ChlorophyllCallback( const gz::msgs::Float &_msg) { @@ -632,6 +650,12 @@ void TethysCommPlugin::PostUpdate( stateMsg.set_temperature_(this->latestTemperature.Celsius()); stateMsg.add_values_(this->latestChlorophyll); + // Battery data + stateMsg.set_batteryvoltage_(this->latestBatteryVoltage); + stateMsg.set_batterycurrent_(this->latestBatteryCurrent); + stateMsg.set_batterycharge_(this->latestBatteryCharge); + stateMsg.set_batterypercentage_(this->latestBatteryPercentage); + // Set Ocean Density stateMsg.set_density_(this->oceanDensity); @@ -676,7 +700,11 @@ void TethysCommPlugin::PostUpdate( << "\tTemperature (C): " << stateMsg.temperature_() << std::endl << "\tSalinity (PSU): " << stateMsg.salinity_() << std::endl << "\tChlorophyll (ug/L): " << stateMsg.values_(0) << std::endl - << "\tPressure (Pa): " << stateMsg.values_(1) << std::endl; + << "\tPressure (Pa): " << stateMsg.values_(1) << std::endl + << "\tBattery Voltage (V): " << stateMsg.batteryvoltage_() << std::endl + << "\tBattery Current (A): " << stateMsg.batterycurrent_() << std::endl + << "\tBattery Charge (Ah): " << stateMsg.batterycharge_() << std::endl + << "\tBattery Percentage (unitless): " << stateMsg.batterypercentage_() << std::endl; this->prevPubPrintTime = _info.simTime; } diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh index 2161e4d4..54863c3e 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.hh +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.hh @@ -71,6 +71,11 @@ namespace tethys public: void TemperatureCallback( const gz::msgs::Double &_msg); + /// Callback function for battery data. + /// \param[in] _msg Battery data + public: void BatteryCallback( + const gz::msgs::BatteryState &_msg); + /// Callback function for chlorophyll sensor data. /// \param[in] _msg Sensor data public: void ChlorophyllCallback( @@ -140,6 +145,10 @@ namespace tethys private: std::string temperatureTopic {"temperature"}; + /// Topic to subscribe to battery data + private: std::string batteryTopic + {"battery/linear_battery/state"}; + /// Topic to subscribe to chlorophyll data private: std::string chlorophyllTopic {"chlorophyll"}; @@ -179,6 +188,18 @@ namespace tethys /// Latest temperature data received from sensor. NaN if not received. private: gz::math::Temperature latestTemperature{std::nanf("")}; + /// Latest battery voltage data received, Nan if not received. + private: double latestBatteryVoltage{std::nanf("")}; + + /// Latest battery current data received, Nan if not received. + private: double latestBatteryCurrent{std::nanf("")}; + + /// Latest battery charge data received, Nan if not received. + private: double latestBatteryCharge{std::nanf("")}; + + /// Latest battery percentage data received, Nan if not received. + private: double latestBatteryPercentage{std::nanf("")}; + /// Latest chlorophyll data received from sensor. NaN if not received. private: float latestChlorophyll{std::nanf("")}; diff --git a/lrauv_system_tests/include/lrauv_system_tests/Subscription.hh b/lrauv_system_tests/include/lrauv_system_tests/Subscription.hh index b9195b01..490e34c6 100644 --- a/lrauv_system_tests/include/lrauv_system_tests/Subscription.hh +++ b/lrauv_system_tests/include/lrauv_system_tests/Subscription.hh @@ -86,6 +86,32 @@ class Subscription return std::move(this->messageHistory); } + /// Current number of messages stored. + /// \return number of messages stored in the + /// messageHistory container. + public: int MessageHistorySize() + { + std::lock_guard lock(this->mutex); + return this->messageHistory.size(); + } + + /// Read the message according to the index. + /// \return message in the messageHistory container + /// based on its index. + public: MessageT GetMessageByIndex(int _index) + { + std::lock_guard lock(this->mutex); + return this->messageHistory[_index]; + } + + /// Reset the messageHistory container by clearing + /// existing messages. + public: void ResetMessageHistory() + { + std::lock_guard lock(this->mutex); + this->messageHistory.clear(); + } + /// Read last message received. /// \note This is a destructive operation. /// \throws std::runtime_error if there is diff --git a/lrauv_system_tests/test/vehicle/CMakeLists.txt b/lrauv_system_tests/test/vehicle/CMakeLists.txt index fbea9318..9d18838d 100644 --- a/lrauv_system_tests/test/vehicle/CMakeLists.txt +++ b/lrauv_system_tests/test/vehicle/CMakeLists.txt @@ -18,6 +18,9 @@ target_link_libraries(test_ahrs gtest_discover_tests(test_ahrs) foreach(_test + test_battery_full_charge + test_battery_half_charge + test_battery_low_charge test_buoyancy_action test_drop_weight test_elevator_action diff --git a/lrauv_system_tests/test/vehicle/test_battery_full_charge.cc b/lrauv_system_tests/test/vehicle/test_battery_full_charge.cc new file mode 100644 index 00000000..bc322aa5 --- /dev/null +++ b/lrauv_system_tests/test/vehicle/test_battery_full_charge.cc @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#include + +#include + +#include + +#include "lrauv_system_tests/TestFixture.hh" + +using namespace gz; +using namespace std::literals::chrono_literals; + +////////////////////////////////////////////////// +/// Test if the battery discharges with time with the specified +/// disacharge power rate, when starting with full charge. +TEST(BatteryTest, TestDischargeFullCharged) +{ + using TestFixture = lrauv_system_tests::TestFixtureWithVehicle; + TestFixture fixture("buoyant_tethys.sdf", "tethys"); + uint64_t iterations = fixture.Step(100u); + + transport::Node node; + lrauv_system_tests::Subscription batterySubscription; + batterySubscription.Subscribe(node, "/model/tethys/battery/linear_battery/state"); + + fixture.Step(1000u); + + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + int n = batterySubscription.MessageHistorySize() - 1; + auto initialMessage = batterySubscription.GetMessageByIndex(0); + double initialCharge = initialMessage.charge(); + double initialVoltage = initialMessage.voltage(); + double initialTime = initialMessage.header().stamp().sec() + + initialMessage.header().stamp().nsec()/1000000000.0; + + /* Plugin started with 100% charge */ + EXPECT_NEAR(initialCharge, 400, 0.2); + + auto finalMessage = batterySubscription.GetMessageByIndex(n); + double finalCharge = finalMessage.charge(); + double finalVoltage = finalMessage.voltage(); + double finalTime = finalMessage.header().stamp().sec() + + finalMessage.header().stamp().nsec()/1000000000.0; + + double dischargePower = (finalVoltage + initialVoltage) * 0.5 * + (finalCharge - initialCharge) * 3600 / (finalTime - initialTime); + EXPECT_NEAR(dischargePower, -28.8, 0.5); +} diff --git a/lrauv_system_tests/test/vehicle/test_battery_half_charge.cc b/lrauv_system_tests/test/vehicle/test_battery_half_charge.cc new file mode 100644 index 00000000..b62a9f5e --- /dev/null +++ b/lrauv_system_tests/test/vehicle/test_battery_half_charge.cc @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#include + +#include + +#include + +#include "lrauv_system_tests/TestFixture.hh" + +using namespace gz; +using namespace std::literals::chrono_literals; + +////////////////////////////////////////////////// +/// Test if the battery discharges with time with the specified +/// disacharge power rate, when starting with half charge. +/// Send recharge start/stop commands and verify the battery behaves +/// accordingly. +TEST(BatteryTest, TestDischargeHalfCharged) +{ + using TestFixture = lrauv_system_tests::TestFixtureWithVehicle; + TestFixture fixture("buoyant_tethys_half_battery.sdf", "tethys"); + uint64_t iterations = fixture.Step(100u); + + transport::Node node; + lrauv_system_tests::Subscription batterySubscription; + batterySubscription.Subscribe(node, "/model/tethys/battery/linear_battery/state"); + + fixture.Step(1000u); + + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + int n = batterySubscription.MessageHistorySize() - 1; + auto initialMessage = batterySubscription.GetMessageByIndex(0); + double initialCharge = initialMessage.charge(); + double initialVoltage = initialMessage.voltage(); + double initialTime = initialMessage.header().stamp().sec() + + initialMessage.header().stamp().nsec()/1000000000.0; + + /* Plugin started wiht 50% charge */ + EXPECT_NEAR(initialCharge, 200, 0.2); + + auto finalMessage = batterySubscription.GetMessageByIndex(n); + double finalCharge = finalMessage.charge(); + double finalVoltage = finalMessage.voltage(); + double finalTime = finalMessage.header().stamp().sec() + + finalMessage.header().stamp().nsec()/1000000000.0; + + /* Check discharge rate when battery is 50% discharged */ + double dischargePower = (finalVoltage + initialVoltage) * 0.5 * + (finalCharge - initialCharge) * 3600 / (finalTime - initialTime); + EXPECT_NEAR(dischargePower, -28.8, 0.5); + + batterySubscription.ResetMessageHistory(); + + /* Test battery recharge command */ + /* Start charging and check if charge increases with time */ + const unsigned int timeout{5000}; + bool result; + msgs::Boolean req; + msgs::Empty rep; + EXPECT_TRUE(node.Request("/model/tethys/battery/linear_battery/recharge/start", req, timeout, rep, result)); + + fixture.Step(1000u); + + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + n = batterySubscription.MessageHistorySize() - 1; + EXPECT_GT(batterySubscription.GetMessageByIndex(n).charge(), finalCharge); + batterySubscription.ResetMessageHistory(); + + /* Stop charging, charge should decrease with time */ + EXPECT_TRUE(node.Request("/model/tethys/battery/linear_battery/recharge/stop", req, timeout, rep, result)); + fixture.Step(1000u); + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + n = batterySubscription.MessageHistorySize() - 1; + EXPECT_LT(batterySubscription.GetMessageByIndex(n).charge(), + batterySubscription.GetMessageByIndex(0).charge()); +} diff --git a/lrauv_system_tests/test/vehicle/test_battery_low_charge.cc b/lrauv_system_tests/test/vehicle/test_battery_low_charge.cc new file mode 100644 index 00000000..3834eb54 --- /dev/null +++ b/lrauv_system_tests/test/vehicle/test_battery_low_charge.cc @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#include + +#include + +#include + +#include + +#include "lrauv_system_tests/TestFixture.hh" + +using namespace gz; +using namespace std::literals::chrono_literals; + +////////////////////////////////////////////////// +/// Test if the battery discharges with time with the specified +/// discharge power rate, when starting with low charge. +/// Send recharge start/stop commands and verify the battery behaves +/// accordingly. +TEST(BatteryTest, TestDischargeLowCharge) +{ + using TestFixture = lrauv_system_tests::VehicleCommandTestFixture; + TestFixture fixture("buoyant_tethys_low_battery.sdf", "tethys"); + uint64_t iterations = fixture.Step(100u); + + transport::Node node; + lrauv_system_tests::Subscription batterySubscription; + batterySubscription.Subscribe(node, "/model/tethys/battery/linear_battery/state"); + + fixture.Step(1000u); + + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + + /* Make sure the battery has drained */ + int n = batterySubscription.MessageHistorySize() - 1; + double initialCharge = batterySubscription.GetMessageByIndex(0).charge(); + double initialVoltage = batterySubscription.GetMessageByIndex(0).voltage(); + + double finalCharge = batterySubscription.GetMessageByIndex(n).charge(); + double finalVoltage = batterySubscription.GetMessageByIndex(n).voltage(); + + EXPECT_NEAR(finalCharge, 0, 1e-3); + EXPECT_NEAR(finalVoltage, 14.4, 0.1); + batterySubscription.ResetMessageHistory(); + /* The battery is now fully discharged */ + + /* Test battery recharge command */ + /* Start charging and check if charge increases with time */ + const unsigned int timeout{5000}; + bool result; + msgs::Boolean req; + msgs::Empty rep; + EXPECT_TRUE(node.Request("/model/tethys/battery/linear_battery/recharge/start", req, timeout, rep, result)); + + fixture.Step(1000u); + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + n = batterySubscription.MessageHistorySize() - 1; + EXPECT_GT(batterySubscription.GetMessageByIndex(n).charge(), finalCharge); + batterySubscription.ResetMessageHistory(); + + /* Stop charging, charge should decrease with time */ + EXPECT_TRUE(node.Request("/model/tethys/battery/linear_battery/recharge/stop", req, timeout, rep, result)); + fixture.Step(1000u); + ASSERT_TRUE(batterySubscription.WaitForMessages(5, 10s)); + n = batterySubscription.MessageHistorySize() - 1; + EXPECT_LT(batterySubscription.GetMessageByIndex(n).charge(), + batterySubscription.GetMessageByIndex(0).charge()); +} diff --git a/lrauv_system_tests/worlds/buoyant_tethys_half_battery.sdf b/lrauv_system_tests/worlds/buoyant_tethys_half_battery.sdf new file mode 100644 index 00000000..60c58d47 --- /dev/null +++ b/lrauv_system_tests/worlds/buoyant_tethys_half_battery.sdf @@ -0,0 +1,383 @@ + + + + + + + 0.0 1.0 1.0 + + + 0.0 0.7 0.8 + + false + + + + 0.02 + 0 + + + + + + + + + + + + + + + 1025 + + 0 + 1.125 + + + + + + + + + + + + + EARTH_WGS84 + ENU + + + 35.5999984741211 + -121.779998779297 + + + + + + + + 0 + 0 + + + 5.5645e-6 22.8758e-6 -42.3884e-6 + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + + 4.5 0 4 0 0.45 3.14 + + + + + + 0.1 + + 3000000 + + + + + + + floating + 5 + 5 + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + false + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + Plot Tethys 3D path + docked_collapsed + + tethys + 0 0 1 + 10000 + 0.5 + + + + Inspector + docked_collapsed + + + + + Visualize science data + docked_collapsed + + + + + Camera controls + docked_collapsed + + + + + docked_collapsed + + + + 6 + 0 + + 50000 + 0 100000 0 0 0 0.32 + 0 1 0 1 + + + + 100 + 0 + + 1 + 0 0 0 0 0 0 + 0.5 0.5 0.5 1 + + + + + Tethys controls + docked_collapsed + + + + + Reference axis + docked_collapsed + + tethys + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + true + + + + + 0 0 1 + + 300000 300000 + + + 1.0 + + + + + + + + + + 0 0 -0.5 0 0 0 + tethys_equipped + + + 200 + + + + + + diff --git a/lrauv_system_tests/worlds/buoyant_tethys_low_battery.sdf b/lrauv_system_tests/worlds/buoyant_tethys_low_battery.sdf new file mode 100644 index 00000000..296c6fec --- /dev/null +++ b/lrauv_system_tests/worlds/buoyant_tethys_low_battery.sdf @@ -0,0 +1,383 @@ + + + + + + + 0.0 1.0 1.0 + + + 0.0 0.7 0.8 + + false + + + + 0.02 + 0 + + + + + + + + + + + + + + + 1025 + + 0 + 1.125 + + + + + + + + + + + + + EARTH_WGS84 + ENU + + + 35.5999984741211 + -121.779998779297 + + + + + + + + 0 + 0 + + + 5.5645e-6 22.8758e-6 -42.3884e-6 + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + + 4.5 0 4 0 0.45 3.14 + + + + + + 0.1 + + 3000000 + + + + + + + floating + 5 + 5 + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + false + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + Plot Tethys 3D path + docked_collapsed + + tethys + 0 0 1 + 10000 + 0.5 + + + + Inspector + docked_collapsed + + + + + Visualize science data + docked_collapsed + + + + + Camera controls + docked_collapsed + + + + + docked_collapsed + + + + 6 + 0 + + 50000 + 0 100000 0 0 0 0.32 + 0 1 0 1 + + + + 100 + 0 + + 1 + 0 0 0 0 0 0 + 0.5 0.5 0.5 1 + + + + + Tethys controls + docked_collapsed + + + + + Reference axis + docked_collapsed + + tethys + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + true + + + + + 0 0 1 + + 300000 300000 + + + 1.0 + + + + + + + + + + 0 0 -0.5 0 0 0 + tethys_equipped + + + 0.01 + + + + + +