From d60016312766fa163d9deebf43d6a5b1fa9d89e2 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 2 Aug 2022 17:33:34 -0700 Subject: [PATCH 01/17] Updated CommsClient to wrap Dataframe Signed-off-by: Aditya --- .../comms/CommsClient.hh | 44 ++++-- .../test/vehicle/test_acoustic_comms.cc | 13 +- .../worlds/acoustic_comms_fixture.sdf | 127 +++++++++++------- 3 files changed, 118 insertions(+), 66 deletions(-) diff --git a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh index a2e2f709..6ee0aef6 100644 --- a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh +++ b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh @@ -25,6 +25,7 @@ #include +#include #include #include "CommsPacket.hh" @@ -32,6 +33,12 @@ #include "lrauv_ignition_plugins/lrauv_acoustic_message.pb.h" +using LRAUVAcousticMessage = + lrauv_ignition_plugins::msgs::LRAUVAcousticMessage; +using MessageType = LRAUVAcousticMessage::MessageType; +static constexpr auto LRAUVAcousticMessageType = + MessageType::LRAUVAcousticMessage_MessageType_Other; + namespace tethys { ////////////////////////////////////////////////// @@ -47,16 +54,13 @@ using Callback = std::function; /// \param[in] _commsPrefix - Optional. Use if your public interface to acoustic /// comms is different. public: CommsClient(uint32_t _address, - Callback _callback, - std::string _commsPrefix=EXTERNAL_COMMS_BUS) : + Callback _callback) : address(_address), callback(std::move(_callback)) { this->transmitter = - this->node.Advertise( - _commsPrefix + "/" + std::to_string(address) + "/tx"); + this->node.Advertise("/broker/msgs"); - this->node.Subscribe( - _commsPrefix + "/" + std::to_string(address) + "/rx", + this->node.Subscribe(std::to_string(address) + "/rx", &CommsClient::ReceivedPacket, this ); @@ -67,15 +71,37 @@ public: CommsClient(uint32_t _address, /// \param[in] _msg - Comms message to be sent public: void SendPacket(const CommsMsg& _msg) { - this->transmitter.Publish(_msg); + // Populate a Dataframe msg based on the + // CommsMsg content. + gz::msgs::Dataframe message; + /* message.set_src_address( */ + /* std::to_string(_msg.from())); */ + /* message.set_dst_address( */ + /* std::to_string(_msg.to())); */ + /* message.set_data(_msg.data()); */ + + std::cout << "----- Dbg sending msg" << std::endl; + + // Publish the Dataframe message + this->transmitter.Publish(message); } ////////////////////////////////////////////////// /// \brief Callback handler /// \param[in] _msg - message received -private: void ReceivedPacket(const CommsMsg& _msg) +private: void ReceivedPacket(const gz::msgs::Dataframe& _msg) { - this->callback(_msg); + std::cout << "----- Dbg received msg" << std::endl; + // Construct a CommsMsg from the received Dataframe msg. + CommsMsg message; + message.set_from( + std::stoi(_msg.src_address())); + message.set_to( + std::stoi(_msg.dst_address())); + message.set_type(LRAUVAcousticMessageType); + message.set_data(_msg.data()); + + this->callback(message); } /// \brief Address which the client binds to diff --git a/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc b/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc index 8c5863c5..1cbbb10c 100644 --- a/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc +++ b/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc @@ -38,13 +38,13 @@ using namespace tethys; using namespace lrauv_system_tests; -using LRAUVAcousticMessage = - lrauv_ignition_plugins::msgs::LRAUVAcousticMessage; +/* using LRAUVAcousticMessage = */ +/* lrauv_ignition_plugins::msgs::LRAUVAcousticMessage; */ using MessageDifferencer = google::protobuf::util::MessageDifferencer; -using MessageType = LRAUVAcousticMessage::MessageType; -static constexpr auto LRAUVAcousticMessageType = - MessageType::LRAUVAcousticMessage_MessageType_Other; +/* using MessageType = LRAUVAcousticMessage::MessageType; */ +/* static constexpr auto LRAUVAcousticMessageType = */ +/* MessageType::LRAUVAcousticMessage_MessageType_Other; */ TEST(AcousticComms, PacketConversions) { @@ -78,6 +78,7 @@ TEST(AcousticComms, BasicSendReceive) constexpr int receiverAddress = 2; CommsClient receiver(receiverAddress, [&](const auto message) { + std::cout << "----- Dbg received msg 2" << std::endl; ASSERT_EQ(message.data(), "test_message"); { std::lock_guard lock(messageArrivalMutex); @@ -93,7 +94,7 @@ TEST(AcousticComms, BasicSendReceive) message.set_data("test_message"); sender.SendPacket(message); - fixture.Step(50u); + fixture.Step(100u); using namespace std::literals::chrono_literals; std::unique_lock lock(messageArrivalMutex); diff --git a/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf b/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf index 4e22d340..9b7daad7 100644 --- a/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf +++ b/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf @@ -51,6 +51,13 @@ + + 2500 + 1500 + + @@ -393,26 +400,32 @@ + + + + + + + + + + + -
2
- simple_acoustic_model - tethys::SimpleAcousticModel - base_link - 2500 - 1500 - 0.000001 - - + filename="gz-sim-comms-endpoint-system" + name="gz::sim::systems::CommsEndpoint">
2
- 0.3 - 1500 - base_link - box1 + 2/rx + + + + + + + + + @@ -442,26 +455,32 @@ + + + + + + + + + + + + filename="gz-sim-comms-endpoint-system" + name="gz::sim::systems::CommsEndpoint">
3
- simple_acoustic_model - tethys::SimpleAcousticModel - base_link - 2500 - 1500 - 0.000001 - - -
3
- 0.3 - 1500 - base_link - box2 + 3/rx + + + + + + + + + @@ -491,26 +510,32 @@ + + + + + + + + + + + -
4
- simple_acoustic_model - tethys::SimpleAcousticModel - base_link - 2500 - 1500 - 0.000001 - - + filename="gz-sim-comms-endpoint-system" + name="gz::sim::systems::CommsEndpoint">
4
- 0.3 - 1500 - base_link - box3 + 4/rx + + + + + + + + + From aaa7daea916ea351b5389ed3409b9bbdc9cc1d27 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 3 Aug 2022 01:03:19 -0700 Subject: [PATCH 02/17] Acoustic comms tests working Signed-off-by: Aditya --- .../comms/CommsClient.hh | 13 ++- .../test/vehicle/test_acoustic_comms.cc | 14 ++- .../worlds/acoustic_comms_fixture.sdf | 87 ++++++------------- 3 files changed, 37 insertions(+), 77 deletions(-) diff --git a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh index 6ee0aef6..345035dc 100644 --- a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh +++ b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh @@ -74,13 +74,11 @@ public: void SendPacket(const CommsMsg& _msg) // Populate a Dataframe msg based on the // CommsMsg content. gz::msgs::Dataframe message; - /* message.set_src_address( */ - /* std::to_string(_msg.from())); */ - /* message.set_dst_address( */ - /* std::to_string(_msg.to())); */ - /* message.set_data(_msg.data()); */ - - std::cout << "----- Dbg sending msg" << std::endl; + message.set_src_address( + std::to_string(_msg.from())); + message.set_dst_address( + std::to_string(_msg.to())); + message.set_data(_msg.data()); // Publish the Dataframe message this->transmitter.Publish(message); @@ -91,7 +89,6 @@ public: void SendPacket(const CommsMsg& _msg) /// \param[in] _msg - message received private: void ReceivedPacket(const gz::msgs::Dataframe& _msg) { - std::cout << "----- Dbg received msg" << std::endl; // Construct a CommsMsg from the received Dataframe msg. CommsMsg message; message.set_from( diff --git a/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc b/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc index 1cbbb10c..7eaa027f 100644 --- a/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc +++ b/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc @@ -38,13 +38,8 @@ using namespace tethys; using namespace lrauv_system_tests; -/* using LRAUVAcousticMessage = */ -/* lrauv_ignition_plugins::msgs::LRAUVAcousticMessage; */ using MessageDifferencer = google::protobuf::util::MessageDifferencer; -/* using MessageType = LRAUVAcousticMessage::MessageType; */ -/* static constexpr auto LRAUVAcousticMessageType = */ -/* MessageType::LRAUVAcousticMessage_MessageType_Other; */ TEST(AcousticComms, PacketConversions) { @@ -69,16 +64,15 @@ TEST(AcousticComms, BasicSendReceive) { TestFixture fixture("acoustic_comms_fixture.sdf"); - constexpr int senderAddress = 1; + constexpr int senderAddress = 2; CommsClient sender(senderAddress, [](const auto){}); bool messageReceived = false; std::mutex messageArrivalMutex; std::condition_variable messageArrival; - constexpr int receiverAddress = 2; + constexpr int receiverAddress = 3; CommsClient receiver(receiverAddress, [&](const auto message) { - std::cout << "----- Dbg received msg 2" << std::endl; ASSERT_EQ(message.data(), "test_message"); { std::lock_guard lock(messageArrivalMutex); @@ -87,6 +81,8 @@ TEST(AcousticComms, BasicSendReceive) messageArrival.notify_all(); }); + fixture.Step(1000u); + LRAUVAcousticMessage message; message.set_to(receiverAddress); message.set_from(senderAddress); @@ -94,7 +90,7 @@ TEST(AcousticComms, BasicSendReceive) message.set_data("test_message"); sender.SendPacket(message); - fixture.Step(100u); + fixture.Step(1000u); using namespace std::literals::chrono_literals; std::unique_lock lock(messageArrivalMutex); diff --git a/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf b/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf index 9b7daad7..ab91bf24 100644 --- a/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf +++ b/lrauv_system_tests/worlds/acoustic_comms_fixture.sdf @@ -400,32 +400,21 @@ - - - - - - - - - - -
2
2/rx - - - - - - - - - + +
2
+ 0.3 + 1500 + base_link + box1 + @@ -455,32 +444,21 @@ - - - - - - - - - - -
3
3/rx - - - - - - - - - + +
3
+ 0.3 + 1500 + base_link + box2 + @@ -510,32 +488,21 @@ - - - - - - - - - - -
4
4/rx - - - - - - - - - + +
4
+ 0.3 + 1500 + base_link + box3 + From fa18500efd1bd661b4b94740bb2d014554c7510c Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 4 Aug 2022 22:00:33 -0700 Subject: [PATCH 03/17] Fixed test_spawn Signed-off-by: Aditya --- .../test/simulation/test_spawn.cc | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/lrauv_system_tests/test/simulation/test_spawn.cc b/lrauv_system_tests/test/simulation/test_spawn.cc index cbc9399f..8fcaf494 100644 --- a/lrauv_system_tests/test/simulation/test_spawn.cc +++ b/lrauv_system_tests/test/simulation/test_spawn.cc @@ -149,16 +149,16 @@ TEST(VehicleSpawnTest, Spawn) std::vector topics; node.TopicList(topics); - EXPECT_TRUE(std::any_of( - topics.begin(), topics.end(), - [](const std::string &s) { - return s == "/comms/external/201/tx"; - })); - EXPECT_TRUE(std::any_of( - topics.begin(), topics.end(), - [](const std::string &s) { - return s == "/comms/external/202/tx"; - })); + /* EXPECT_TRUE(std::any_of( */ + /* topics.begin(), topics.end(), */ + /* [](const std::string &s) { */ + /* return s == "/comms/external/201/tx"; */ + /* })); */ + /* EXPECT_TRUE(std::any_of( */ + /* topics.begin(), topics.end(), */ + /* [](const std::string &s) { */ + /* return s == "/comms/external/202/tx"; */ + /* })); */ constexpr double tightTol{1e-5}; From 240c0317840fb3eeff79dc38783990d2d477b15f Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 5 Aug 2022 12:29:07 -0700 Subject: [PATCH 04/17] Added msg types to dataframe msg Signed-off-by: Aditya --- .../comms/CommsClient.hh | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh index 345035dc..14a5fafa 100644 --- a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh +++ b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh @@ -38,6 +38,10 @@ using LRAUVAcousticMessage = using MessageType = LRAUVAcousticMessage::MessageType; static constexpr auto LRAUVAcousticMessageType = MessageType::LRAUVAcousticMessage_MessageType_Other; +auto RangeRequest = + MessageType::LRAUVAcousticMessage_MessageType_RangeRequest; +auto RangeResponse = + MessageType::LRAUVAcousticMessage_MessageType_RangeResponse; namespace tethys { @@ -80,6 +84,16 @@ public: void SendPacket(const CommsMsg& _msg) std::to_string(_msg.to())); message.set_data(_msg.data()); + // 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"); + // Publish the Dataframe message this->transmitter.Publish(message); } @@ -95,9 +109,23 @@ private: void ReceivedPacket(const gz::msgs::Dataframe& _msg) std::stoi(_msg.src_address())); message.set_to( std::stoi(_msg.dst_address())); - message.set_type(LRAUVAcousticMessageType); message.set_data(_msg.data()); + // Set message type. + for (int i = 0; i < _msg.header().data_size(); i++) + { + 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); + } + } + this->callback(message); } From e6352b6f1e1f26ec0f18f9312c23447dc3c90717 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 5 Aug 2022 13:26:11 -0700 Subject: [PATCH 05/17] test_range_bearing working Signed-off-by: Aditya --- lrauv_description/models/tethys_equipped/model.sdf | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index e84b1c40..b2ce549a 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -373,14 +373,10 @@ + filename="gz-sim-comms-endpoint-system" + name="gz::sim::systems::CommsEndpoint">
1
- simple_acoustic_model - tethys::SimpleAcousticModel - acoustic_transponder - 2500 - 1500 + 1/rx Date: Fri, 5 Aug 2022 17:14:23 -0700 Subject: [PATCH 06/17] Clean up test_acoustic_comms Signed-off-by: Aditya --- lrauv_system_tests/test/vehicle/test_acoustic_comms.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc b/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc index 7eaa027f..2397c4c2 100644 --- a/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc +++ b/lrauv_system_tests/test/vehicle/test_acoustic_comms.cc @@ -64,13 +64,13 @@ TEST(AcousticComms, BasicSendReceive) { TestFixture fixture("acoustic_comms_fixture.sdf"); - constexpr int senderAddress = 2; + constexpr int senderAddress = 1; CommsClient sender(senderAddress, [](const auto){}); bool messageReceived = false; std::mutex messageArrivalMutex; std::condition_variable messageArrival; - constexpr int receiverAddress = 3; + constexpr int receiverAddress = 2; CommsClient receiver(receiverAddress, [&](const auto message) { ASSERT_EQ(message.data(), "test_message"); @@ -81,7 +81,7 @@ TEST(AcousticComms, BasicSendReceive) messageArrival.notify_all(); }); - fixture.Step(1000u); + fixture.Step(50u); LRAUVAcousticMessage message; message.set_to(receiverAddress); @@ -90,7 +90,7 @@ TEST(AcousticComms, BasicSendReceive) message.set_data("test_message"); sender.SendPacket(message); - fixture.Step(1000u); + fixture.Step(50u); using namespace std::literals::chrono_literals; std::unique_lock lock(messageArrivalMutex); From cf4c67b26811b8f2208c50b8f43f577bd0c7214a Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 5 Aug 2022 17:41:57 -0700 Subject: [PATCH 07/17] Updated included worlds Signed-off-by: Aditya --- lrauv_ignition_plugins/worlds/multi_lrauv.sdf | 6 ++++-- lrauv_ignition_plugins/worlds/star_world.sdf | 9 ++++++--- lrauv_system_tests/worlds/star_world.sdf | 12 ++++++++---- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/lrauv_ignition_plugins/worlds/multi_lrauv.sdf b/lrauv_ignition_plugins/worlds/multi_lrauv.sdf index 9d5c9df3..6e4e3c0a 100644 --- a/lrauv_ignition_plugins/worlds/multi_lrauv.sdf +++ b/lrauv_ignition_plugins/worlds/multi_lrauv.sdf @@ -145,8 +145,9 @@ /model/triton/drop_weight - +
2
+ 2/rx
2
@@ -188,8 +189,9 @@ /model/daphne/drop_weight - +
3
+ 3/rx
3
diff --git a/lrauv_ignition_plugins/worlds/star_world.sdf b/lrauv_ignition_plugins/worlds/star_world.sdf index e1e083b9..ba3f0de8 100644 --- a/lrauv_ignition_plugins/worlds/star_world.sdf +++ b/lrauv_ignition_plugins/worlds/star_world.sdf @@ -110,8 +110,9 @@ /model/tethys2/drop_weight - +
3
+ 3/rx
3
@@ -151,8 +152,9 @@ /model/tethys3/drop_weight - +
4
+ 4/rx
4
@@ -192,8 +194,9 @@ /model/tethys4/drop_weight - +
5
+ 5/rx
5
diff --git a/lrauv_system_tests/worlds/star_world.sdf b/lrauv_system_tests/worlds/star_world.sdf index c5575fe7..34f01216 100644 --- a/lrauv_system_tests/worlds/star_world.sdf +++ b/lrauv_system_tests/worlds/star_world.sdf @@ -103,8 +103,9 @@ /model/tethys1/drop_weight - +
1
+ 1/rx
1
@@ -144,8 +145,9 @@ /model/tethys2/drop_weight - +
2
+ 2/rx
2
@@ -185,8 +187,9 @@ /model/tethys3/drop_weight - +
3
+ 3/rx
3
@@ -226,8 +229,9 @@ /model/tethys4/drop_weight - +
4
+ 4/rx
4
From 0bc72f029a4da547857007fc653216d2652a9abc Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 5 Aug 2022 17:48:19 -0700 Subject: [PATCH 08/17] Updated included worlds Signed-off-by: Aditya --- lrauv_ignition_plugins/src/WorldCommPlugin.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.cc b/lrauv_ignition_plugins/src/WorldCommPlugin.cc index 695fc8bc..4f7d1d6a 100644 --- a/lrauv_ignition_plugins/src/WorldCommPlugin.cc +++ b/lrauv_ignition_plugins/src/WorldCommPlugin.cc @@ -300,8 +300,9 @@ std::string WorldCommPlugin::TethysSdfString(const lrauv_ignition_plugins::msgs: /model/)" + _id + R"(/drop_weight - +
)" + _acommsAddress + R"(
+ )" + _acommsAddress + R"(/rx From fc8ce503ab1f3117f8b04791511122b464c42ca7 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 5 Aug 2022 18:26:43 -0700 Subject: [PATCH 09/17] Updated plugin in worlds Signed-off-by: Aditya --- lrauv_ignition_plugins/worlds/multi_lrauv.sdf | 6 ++++++ lrauv_ignition_plugins/worlds/star_world.sdf | 6 ++++++ lrauv_system_tests/worlds/star_world.sdf | 6 ++++++ 3 files changed, 18 insertions(+) diff --git a/lrauv_ignition_plugins/worlds/multi_lrauv.sdf b/lrauv_ignition_plugins/worlds/multi_lrauv.sdf index 6e4e3c0a..fd6d0529 100644 --- a/lrauv_ignition_plugins/worlds/multi_lrauv.sdf +++ b/lrauv_ignition_plugins/worlds/multi_lrauv.sdf @@ -43,6 +43,12 @@ name="gz::sim::systems::Buoyancy"> 1025 + + 2500 + 1500 + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + + + + 2500 + 1000 + + + + 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 + + + + -5 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator + + + + 0 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + +
2
+ 2/rx + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + 15 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + triton + + + +
1
+ 1/rx + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + triton + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + -15 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + daphne + + + +
3
+ 3/rx + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + daphne + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + + From feeead6f2803ab83e2944dfde26a5bb02ca96c17 Mon Sep 17 00:00:00 2001 From: Aditya Date: Tue, 6 Sep 2022 10:43:03 -0700 Subject: [PATCH 14/17] Use static constexpr Signed-off-by: Aditya --- .../include/lrauv_ignition_plugins/comms/CommsClient.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh index ac35eb3f..e22fbf70 100644 --- a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh +++ b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh @@ -37,9 +37,9 @@ 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; namespace tethys From 5e94ed4e04d143df6bbfeb7126d64e157ca960b5 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 7 Sep 2022 09:34:41 -0700 Subject: [PATCH 15/17] Removed empty lines Signed-off-by: Aditya --- lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf | 3 --- 1 file changed, 3 deletions(-) diff --git a/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf b/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf index 7ed186c7..c95ecd52 100644 --- a/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf +++ b/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf @@ -266,7 +266,6 @@ -632.698957 0 - @@ -368,9 +367,7 @@ -632.698957 0 - - From 75fe73412f8c588191bfe05640115556d6abb973 Mon Sep 17 00:00:00 2001 From: Aditya Date: Wed, 7 Sep 2022 16:30:13 -0700 Subject: [PATCH 16/17] Minor refactoring Signed-off-by: Aditya --- .../comms/CommsClient.hh | 46 +++++++++++++------ 1 file changed, 33 insertions(+), 13 deletions(-) diff --git a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh index e22fbf70..56c22e84 100644 --- a/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh +++ b/lrauv_ignition_plugins/include/lrauv_ignition_plugins/comms/CommsClient.hh @@ -24,11 +24,11 @@ #define __LRAUV_IGNITION_PLUGINS_COMMS_COMMSCLIENT_HH__ #include +#include #include #include -/* #include "CommsPacket.hh" */ #include "lrauv_ignition_plugins/lrauv_acoustic_message.pb.h" @@ -42,6 +42,20 @@ static constexpr auto RangeRequest = static constexpr auto RangeResponse = MessageType::LRAUVAcousticMessage_MessageType_RangeResponse; +std::unordered_map +msgToString = { + {LRAUVAcousticMessageType, "LRAUVAcousticMessageType"}, + {RangeRequest, "RangeRequest"}, + {RangeResponse, "RangeResponse"} +}; + +std::unordered_map +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; + } } } From 1aeb14732c83d553b533dcc38e1c846aa3369609 Mon Sep 17 00:00:00 2001 From: Aditya Date: Thu, 8 Sep 2022 12:02:13 -0700 Subject: [PATCH 17/17] Refactored sdf file, split test in a different file Signed-off-by: Aditya --- .../models/tethys_equipped/model.sdf | 18 +- .../test/vehicle/CMakeLists.txt | 12 + .../test_acoustic_comms_multi_vehicle.cc | 119 ++++++ .../worlds/acoustic_comms_multi_vehicle.sdf | 352 +++--------------- 4 files changed, 188 insertions(+), 313 deletions(-) create mode 100644 lrauv_system_tests/test/vehicle/test_acoustic_comms_multi_vehicle.cc diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index 080a78a6..b20edd04 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -245,19 +245,19 @@ horizontal_fins_joint 0.1 vertical_fins_joint 0.1 tethys propeller_joint @@ -269,7 +269,7 @@ true battery_joint true @@ -278,7 +278,7 @@ 1025 4.13 @@ -297,7 +297,7 @@ 1025 4.13 @@ -349,7 +349,7 @@ 0 buoyancy_engine tethys @@ -365,7 +365,7 @@ 0.000003 base_link __model__ @@ -393,7 +393,7 @@ - linear_battery 14.4 diff --git a/lrauv_system_tests/test/vehicle/CMakeLists.txt b/lrauv_system_tests/test/vehicle/CMakeLists.txt index 33545f3c..a90554ed 100644 --- a/lrauv_system_tests/test/vehicle/CMakeLists.txt +++ b/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 diff --git a/lrauv_system_tests/test/vehicle/test_acoustic_comms_multi_vehicle.cc b/lrauv_system_tests/test/vehicle/test_acoustic_comms_multi_vehicle.cc new file mode 100644 index 00000000..ce255036 --- /dev/null +++ b/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 + +#include +#include +#include + +#include + +#include +#include + +#include + +#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 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 lock(messageArrivalMutexTethys); + messageReceivedTethys = true; + receivedTimeTethys = std::chrono::system_clock::now(); + } + messageArrivalTethys.notify_all(); + }); + + std::chrono::time_point 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 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 lockTethys(messageArrivalMutexTethys); + EXPECT_TRUE(messageArrivalTethys.wait_for( + lockTethys, 5s, [&] { return messageReceivedTethys; })); + + std::unique_lock lockDaphne(messageArrivalMutexDaphne); + EXPECT_TRUE(messageArrivalDaphne.wait_for( + lockDaphne, 5s, [&] { return messageReceivedDaphne; })); + + std::chrono::duration diffTethys = receivedTimeTethys - startTime; + std::chrono::duration 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); +} diff --git a/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf b/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf index c95ecd52..cbde15b0 100644 --- a/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf +++ b/lrauv_system_tests/worlds/acoustic_comms_multi_vehicle.sdf @@ -1,10 +1,15 @@ - - + + 0.0 1.0 1.0 + + 0.0 0.7 0.8 @@ -24,22 +29,20 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + filename="gz-sim-imu-system" + name="gz::sim::systems::Imu"> - - 1000 + filename="gz-sim-magnetometer-system" + name="gz::sim::systems::Magnetometer"> - + filename="gz-sim-buoyancy-system" + name="gz::sim::systems::Buoyancy"> + 1025 - @@ -47,6 +50,11 @@ 1000 + + + true 0 0 10 0 0 0 @@ -63,310 +71,46 @@ -5 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator + turbidity_generator - 0 0 1 0 0 1.57 - https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV - - - -
2
- 2/rx - - - - - horizontal_fins_joint - 0.1 - - - - vertical_fins_joint - 0.1 - - - - tethys - propeller_joint - 0.004422 - 1000 - 0.2 - - - - - - - 1000 - 4.13 - -1.1 - 0.2 - 0.03 - 0.17 - 0 - 0.0244 - 0 1 0 - 1 0 0 - vertical_fins - 0 0 0 - - - - - 1000 - 4.13 - -1.1 - 0.2 - 0.03 - 0.17 - 0 - 0.0244 - 0 0 1 - 1 0 0 - horizontal_fins - 0 0 0 - - - - base_link - -4.876161 - -126.324739 - -126.324739 - 0 - -33.46 - -33.46 - -6.2282 - 0 - -601.27 - 0 - -601.27 - 0 - -0.1916 - 0 - -632.698957 - 0 - -632.698957 - 0 - + -15 0 1 0 0 0 + tethys_equipped + triton + + +
1
+ 1/rx + + - 15 0 1 0 0 1.57 - https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV - triton - - - -
1
- 1/rx - - - - - horizontal_fins_joint - 0.1 - - - - vertical_fins_joint - 0.1 - - - - triton - propeller_joint - 0.004422 - 1000 - 0.2 - - - - - - - 1000 - 4.13 - -1.1 - 0.2 - 0.03 - 0.17 - 0 - 0.0244 - 0 1 0 - 1 0 0 - vertical_fins - 0 0 0 - - - - - 1000 - 4.13 - -1.1 - 0.2 - 0.03 - 0.17 - 0 - 0.0244 - 0 0 1 - 1 0 0 - horizontal_fins - 0 0 0 - - - - base_link - -4.876161 - -126.324739 - -126.324739 - 0 - -33.46 - -33.46 - -6.2282 - 0 - -601.27 - 0 - -601.27 - 0 - -0.1916 - 0 - -632.698957 - 0 - -632.698957 - 0 - + 0 0 1 0 0 0 + tethys_equipped + tethys + + + +
2
+ 2/rx + + - -15 0 1 0 0 1.57 - https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + 15 0 1 0 0 0 + tethys_equipped daphne - - -
3
- 3/rx - - - - - horizontal_fins_joint - 0.1 - - - - vertical_fins_joint - 0.1 - - - - daphne - propeller_joint - 0.004422 - 1000 - 0.2 - - - - - - - 1000 - 4.13 - -1.1 - 0.2 - 0.03 - 0.17 - 0 - 0.0244 - 0 1 0 - 1 0 0 - vertical_fins - 0 0 0 - - - - - 1000 - 4.13 - -1.1 - 0.2 - 0.03 - 0.17 - 0 - 0.0244 - 0 0 1 - 1 0 0 - horizontal_fins - 0 0 0 - - - - base_link - -4.876161 - -126.324739 - -126.324739 - 0 - -33.46 - -33.46 - -6.2282 - 0 - -601.27 - 0 - -601.27 - 0 - -0.1916 - 0 - -632.698957 - 0 - -632.698957 - 0 - + + +
3
+ 3/rx + +