Added integration test using multiple lrauv vehicles

lrauv_description/models/tethys_equipped/model.sdf

@@ -245,19 +245,19 @@
  </experimental:params>
  <!-- Joint controllers -->
  <plugin
- filename="ignition-gazebo-joint-position-controller-system"
+ filename="gz-sim-joint-position-controller-system"
  name="gz::sim::systems::JointPositionController">
  <joint_name>horizontal_fins_joint</joint_name>
  <p_gain>0.1</p_gain>
  </plugin>
  <plugin
- filename="ignition-gazebo-joint-position-controller-system"
+ filename="gz-sim-joint-position-controller-system"
  name="gz::sim::systems::JointPositionController">
  <joint_name>vertical_fins_joint</joint_name>
  <p_gain>0.1</p_gain>
  </plugin>
  <plugin
- filename="ignition-gazebo-thruster-system"
+ filename="gz-sim-thruster-system"
  name="gz::sim::systems::Thruster">
  <namespace>tethys</namespace>
  <joint_name>propeller_joint</joint_name>
@@ -269,7 +269,7 @@
  <use_angvel_cmd>true</use_angvel_cmd>
  </plugin>
  <plugin
- filename="ignition-gazebo-joint-position-controller-system"
+ filename="gz-sim-joint-position-controller-system"
  name="gz::sim::systems::JointPositionController">
  <joint_name>battery_joint</joint_name>
  <use_velocity_commands>true</use_velocity_commands>
@@ -278,7 +278,7 @@
  <!-- Lift and drag -->
  <!-- Vertical fin -->
  <plugin
- filename="ignition-gazebo-lift-drag-system"
+ filename="gz-sim-lift-drag-system"
  name="gz::sim::systems::LiftDrag">
  <air_density>1025</air_density>
  <cla>4.13</cla>
@@ -297,7 +297,7 @@
  </plugin>
  <!-- Horizontal fin -->
  <plugin
- filename="ignition-gazebo-lift-drag-system"
+ filename="gz-sim-lift-drag-system"
  name="gz::sim::systems::LiftDrag">
  <air_density>1025</air_density>
  <cla>4.13</cla>
@@ -349,7 +349,7 @@
  <nR>0</nR>
  </plugin>
  <plugin
- filename="ignition-gazebo-buoyancy-engine-system"
+ filename="gz-sim-buoyancy-engine-system"
  name="gz::sim::systems::BuoyancyEngine">
  <link_name>buoyancy_engine</link_name>
  <namespace>tethys</namespace>
@@ -365,7 +365,7 @@
  <max_inflation_rate>0.000003</max_inflation_rate>
  </plugin>
  <plugin
- filename="ignition-gazebo-detachable-joint-system"
+ filename="gz-sim-detachable-joint-system"
  name="gz::sim::systems::DetachableJoint">
  <parent_link>base_link</parent_link>
  <child_model>__model__</child_model>
@@ -393,7 +393,7 @@
  <!-- The consists of 62 batteries of NH2034HD34 Lithium Ion batteries. -->
  <!-- Datasheet: https:/osrf/lrauv/files/8614587/NH2034HD34_spec_v1.5.pdf -->
  <!-- Reference comment : https:/osrf/lrauv/issues/201#issuecomment-1116729239 -->
- <plugin filename="ignition-gazebo-linearbatteryplugin-system"
+ <plugin filename="gz-sim-linearbatteryplugin-system"
  name="gz::sim::systems::LinearBatteryPlugin">
  <battery_name>linear_battery</battery_name>
  <voltage>14.4</voltage>

lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh

@@ -24,11 +24,11 @@
 #define __LRAUV_IGNITION_PLUGINS_COMMS_COMMSCLIENT_HH__
 
 #include <functional>
+#include <unordered_map>
 
 #include <gz/msgs.hh>
 #include <gz/transport/Node.hh>
 
-/* #include "CommsPacket.hh" */
 
 #include "lrauv_ignition_plugins/lrauv_acoustic_message.pb.h"
 
@@ -37,11 +37,25 @@ using LRAUVAcousticMessage =
 using MessageType = LRAUVAcousticMessage::MessageType;
 static constexpr auto LRAUVAcousticMessageType =
  MessageType::LRAUVAcousticMessage_MessageType_Other;
-auto RangeRequest =
+static constexpr auto RangeRequest =
  MessageType::LRAUVAcousticMessage_MessageType_RangeRequest;
-auto RangeResponse =
+static constexpr auto RangeResponse =
  MessageType::LRAUVAcousticMessage_MessageType_RangeResponse;
 
+std::unordered_map<MessageType, std::string>
+msgToString = {
+ {LRAUVAcousticMessageType, "LRAUVAcousticMessageType"},
+ {RangeRequest, "RangeRequest"},
+ {RangeResponse, "RangeResponse"}
+};
+
+std::unordered_map<std::string, MessageType>
+stringToMsg = {
+ {"LRAUVAcousticMessageType", LRAUVAcousticMessageType},
+ {"RangeRequest", RangeRequest},
+ {"RangeResponse", RangeResponse}
+};
+
 namespace tethys
 {
 //////////////////////////////////////////////////
@@ -86,12 +100,15 @@ public: void SendPacket(const CommsMsg& _msg)
  // Set message type
  auto *frame = message.mutable_header()->add_data();
  frame->set_key("msg_type");
- if (_msg.type() == LRAUVAcousticMessageType)
- frame->add_value("LRAUVAcousticMessageType");
- else if (_msg.type() == RangeRequest)
- frame->add_value("RangeRequest");
- else if (_msg.type() == RangeResponse)
- frame->add_value("RangeResponse");
+ if (msgToString.count(_msg.type()) != 0)
+ {
+ frame->add_value(msgToString[_msg.type()]);
+ }
+ else
+ {
+ std::cerr << "Message being sent does not contain a type"
+ << std::endl;
+ }
 
  // Publish the Dataframe message
  this->transmitter.Publish(message);
@@ -116,12 +133,15 @@ private: void ReceivedPacket(const gz::msgs::Dataframe& _msg)
  if (_msg.header().data(i).key() == "msg_type")
  {
  auto type = _msg.header().data(i).value().Get(0);
- if (type == "LRAUVAcousticMessageType")
- message.set_type(LRAUVAcousticMessageType);
- else if (type == "RangeResponse")
- message.set_type(RangeResponse);
- else if (type == "RangeRequest")
- message.set_type(RangeRequest);
+ if (stringToMsg.count(type) != 0)
+ {
+ message.set_type(stringToMsg[type]);
+ }
+ else
+ {
+ std::cerr << "Received message does not contain a type"
+ << std::endl;
+ }
  }
  }
 

lrauv_system_tests/test/vehicle/CMakeLists.txt

@@ -10,6 +10,18 @@ target_link_libraries(test_acoustic_comms
 )
 gtest_discover_tests(test_acoustic_comms)
 
+add_executable(test_acoustic_comms_multi_vehicle test_acoustic_comms_multi_vehicle.cc)
+target_include_directories(test_acoustic_comms_multi_vehicle
+ PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
+target_link_libraries(test_acoustic_comms_multi_vehicle
+ PUBLIC gtest_main
+ PRIVATE
+ ${PROJECT_NAME}_support ${IGN_GAZEBO_TARGET}
+ lrauv_ignition_plugins::acoustic_comms_support
+ lrauv_ignition_plugins::lrauv_acoustic_message
+)
+gtest_discover_tests(test_acoustic_comms_multi_vehicle)
+
 add_executable(test_ahrs test_ahrs.cc)
 target_link_libraries(test_ahrs
  PUBLIC gtest_main

lrauv_system_tests/test/vehicle/test_acoustic_comms.cc

@@ -97,3 +97,81 @@ TEST(AcousticComms, BasicSendReceive)
  EXPECT_TRUE(messageArrival.wait_for(
  lock, 5s, [&] { return messageReceived; }));
 }
+
+TEST(AcousticComms, MultiVehicleTest)
+{
+ TestFixture fixture("acoustic_comms_multi_vehicle.sdf");
+
+ constexpr int senderAddressTriton = 1;
+ CommsClient senderTriton(senderAddressTriton, [](const auto){});
+
+ // Monitor messages on comms for Tethys and Daphne
+ std::chrono::time_point<std::chrono::system_clock> receivedTimeTethys;
+ bool messageReceivedTethys = false;
+ std::mutex messageArrivalMutexTethys;
+ std::condition_variable messageArrivalTethys;
+ constexpr int receiverAddressTethys = 2;
+ CommsClient receiverTethys(receiverAddressTethys, [&](const auto message)
+ {
+ ASSERT_EQ(message.data(), "test_message");
+ {
+ std::lock_guard<std::mutex> lock(messageArrivalMutexTethys);
+ messageReceivedTethys = true;
+ receivedTimeTethys = std::chrono::system_clock::now();
+ }
+ messageArrivalTethys.notify_all();
+ });
+
+ std::chrono::time_point<std::chrono::system_clock> receivedTimeDaphne;
+ bool messageReceivedDaphne = false;
+ std::mutex messageArrivalMutexDaphne;
+ std::condition_variable messageArrivalDaphne;
+ constexpr int receiverAddressDaphne = 3;
+ CommsClient receiverDaphne(receiverAddressDaphne, [&](const auto message)
+ {
+ ASSERT_EQ(message.data(), "test_message");
+ {
+ std::lock_guard<std::mutex> lock(messageArrivalMutexDaphne);
+ messageReceivedDaphne = true;
+ receivedTimeDaphne = std::chrono::system_clock::now();
+ }
+ messageArrivalDaphne.notify_all();
+ });
+
+ fixture.Step(50u);
+
+ // Send the messages from Triton
+ LRAUVAcousticMessage message;
+ message.set_to(receiverAddressTethys);
+ message.set_from(senderAddressTriton);
+ message.set_type(LRAUVAcousticMessageType);
+ message.set_data("test_message");
+ senderTriton.SendPacket(message);
+
+ message.set_to(receiverAddressDaphne);
+ senderTriton.SendPacket(message);
+
+ auto startTime = std::chrono::system_clock::now();
+
+ fixture.Step(50u);
+
+ // Check if the messages were received
+ using namespace std::literals::chrono_literals;
+
+ std::unique_lock<std::mutex> lockTethys(messageArrivalMutexTethys);
+ EXPECT_TRUE(messageArrivalTethys.wait_for(
+ lockTethys, 5s, [&] { return messageReceivedTethys; }));
+
+ std::unique_lock<std::mutex> lockDaphne(messageArrivalMutexDaphne);
+ EXPECT_TRUE(messageArrivalDaphne.wait_for(
+ lockDaphne, 5s, [&] { return messageReceivedDaphne; }));
+
+ std::chrono::duration<double> diffTethys = receivedTimeTethys - startTime;
+ std::chrono::duration<double> diffDaphne = receivedTimeDaphne - startTime;
+
+ // Since the distance between Triton and Daphne is twice the distance between
+ // Triton and Tethys, the ratio of time taken by the comms signal to reach them
+ // should be in the same ratio.
+ EXPECT_NEAR(diffDaphne.count() / diffTethys.count(), 2.0, 0.1);
+}
+

lrauv_system_tests/test/vehicle/test_acoustic_comms_multi_vehicle.cc

@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) 2021 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 <gtest/gtest.h>
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+
+#include <lrauv_ignition_plugins/lrauv_acoustic_message.pb.h>
+
+#include <lrauv_ignition_plugins/comms/CommsClient.hh>
+#include <lrauv_ignition_plugins/comms/CommsPacket.hh>
+
+#include <google/protobuf/util/message_differencer.h>
+
+#include "lrauv_system_tests/TestFixture.hh"
+
+using namespace tethys;
+using namespace lrauv_system_tests;
+
+using MessageDifferencer =
+ google::protobuf::util::MessageDifferencer;
+
+TEST(AcousticComms, MultiVehicleTest)
+{
+ TestFixture fixture("acoustic_comms_multi_vehicle.sdf");
+
+ constexpr int senderAddressTriton = 1;
+ CommsClient senderTriton(senderAddressTriton, [](const auto){});
+
+ // Monitor messages on comms for Tethys and Daphne
+ std::chrono::time_point<std::chrono::system_clock> receivedTimeTethys;
+ bool messageReceivedTethys = false;
+ std::mutex messageArrivalMutexTethys;
+ std::condition_variable messageArrivalTethys;
+ constexpr int receiverAddressTethys = 2;
+ CommsClient receiverTethys(receiverAddressTethys, [&](const auto message)
+ {
+ ASSERT_EQ(message.data(), "test_message");
+ {
+ std::lock_guard<std::mutex> lock(messageArrivalMutexTethys);
+ messageReceivedTethys = true;
+ receivedTimeTethys = std::chrono::system_clock::now();
+ }
+ messageArrivalTethys.notify_all();
+ });
+
+ std::chrono::time_point<std::chrono::system_clock> receivedTimeDaphne;
+ bool messageReceivedDaphne = false;
+ std::mutex messageArrivalMutexDaphne;
+ std::condition_variable messageArrivalDaphne;
+ constexpr int receiverAddressDaphne = 3;
+ CommsClient receiverDaphne(receiverAddressDaphne, [&](const auto message)
+ {
+ ASSERT_EQ(message.data(), "test_message");
+ {
+ std::lock_guard<std::mutex> lock(messageArrivalMutexDaphne);
+ messageReceivedDaphne = true;
+ receivedTimeDaphne = std::chrono::system_clock::now();
+ }
+ messageArrivalDaphne.notify_all();
+ });
+
+ fixture.Step(50u);
+
+ // Send the messages from Triton
+ LRAUVAcousticMessage message;
+ message.set_to(receiverAddressTethys);
+ message.set_from(senderAddressTriton);
+ message.set_type(LRAUVAcousticMessageType);
+ message.set_data("test_message");
+ senderTriton.SendPacket(message);
+
+ message.set_to(receiverAddressDaphne);
+ senderTriton.SendPacket(message);
+
+ auto startTime = std::chrono::system_clock::now();
+
+ fixture.Step(50u);
+
+ // Check if the messages were received
+ using namespace std::literals::chrono_literals;
+
+ std::unique_lock<std::mutex> lockTethys(messageArrivalMutexTethys);
+ EXPECT_TRUE(messageArrivalTethys.wait_for(
+ lockTethys, 5s, [&] { return messageReceivedTethys; }));
+
+ std::unique_lock<std::mutex> lockDaphne(messageArrivalMutexDaphne);
+ EXPECT_TRUE(messageArrivalDaphne.wait_for(
+ lockDaphne, 5s, [&] { return messageReceivedDaphne; }));
+
+ std::chrono::duration<double> diffTethys = receivedTimeTethys - startTime;
+ std::chrono::duration<double> diffDaphne = receivedTimeDaphne - startTime;
+
+ // Since the distance between Triton and Daphne is twice the distance between
+ // Triton and Tethys, the ratio of time taken by the comms signal to reach them
+ // should be in the same ratio.
+ EXPECT_NEAR(diffDaphne.count() / diffTethys.count(), 2.0, 0.1);
+}