Merge branch 'arjo/feat/envrionment_sensors_rebased' into arjo/feat/e…

…nvironment_visuals

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

examples/standalone/acoustic_comms_demo/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+
+project(gz-sim-acoustic-comms-demo)
+
+find_package(gz-transport12 QUIET REQUIRED OPTIONAL_COMPONENTS log)
+set(GZ_TRANSPORT_VER ${gz-transport12_VERSION_MAJOR})
+
+find_package(gz-sim7 REQUIRED)
+set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
+
+add_executable(acoustic_comms_demo acoustic_comms_demo.cc)
+target_link_libraries(acoustic_comms_demo
+  gz-transport${GZ_TRANSPORT_VER}::core
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})

examples/standalone/acoustic_comms_demo/README.md

@@ -0,0 +1,31 @@
+# Multi-LRAUV Acoustic comms example
+
+This example shows the usage of the Acoustic comms plugin on
+multiple autonomous underwater vehicles (AUV) with buoyancy, lift drag, and
+hydrodynamics plugins. The multiple vehicles are differentiated by namespaces.
+
+It consists of 3 vehicles,
+Triton, Tethys, and Daphne floating side by side. Triton sends
+a move command using acoustic comms to the other 2 vehicles,
+which start moving on receiving the command. The speed of sound
+is purposely slowed down here to show that the middle vehicle (Tethys)
+will receive the signal and start moving before Daphne.
+
+## Build Instructions
+
+From this directory, run the following to compile:
+
+    mkdir build
+    cd build
+    cmake ..
+    make
+
+## Execute Instructions
+
+From the `build` directory, run Gazebo Sim and the example controller:
+
+    gz sim -r ../../../worlds/acoustic_comms_demo.sdf
+    ./acoustic_comms_demo
+
+It can be seen visually that one of the vehicles (Triton) starts moving
+immediately, then afer a while Tethys will start moving, and then finally Daphne.

examples/standalone/acoustic_comms_demo/acoustic_comms_demo.cc

@@ -0,0 +1,135 @@
+/*
+ * 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.
+ *
+ */
+
+/*
+ * Check README for detailed instructions.
+ * Usage:
+ *   $ gz sim -r worlds/acoustic_comms_demo.sdf
+ *   $ ./acoustic_comms_demo
+ */
+
+#include <atomic>
+#include <chrono>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <gz/msgs.hh>
+#include <gz/transport.hh>
+
+using namespace gz;
+
+int main(int argc, char** argv)
+{
+  transport::Node node;
+
+  auto propellerTopicTriton =
+    gz::transport::TopicUtils::AsValidTopic(
+      "/model/triton/joint/propeller_joint/cmd_pos");
+  auto propellerPubTriton =
+    node.Advertise<gz::msgs::Double>(propellerTopicTriton);
+
+  auto propellerTopicTethys =
+    gz::transport::TopicUtils::AsValidTopic(
+      "/model/tethys/joint/propeller_joint/cmd_pos");
+  auto propellerPubTethys =
+    node.Advertise<gz::msgs::Double>(propellerTopicTethys);
+
+  auto propellerTopicDaphne =
+    gz::transport::TopicUtils::AsValidTopic(
+      "/model/daphne/joint/propeller_joint/cmd_pos");
+  auto propellerPubDaphne =
+    node.Advertise<gz::msgs::Double>(propellerTopicDaphne);
+
+  double propellerCmdTriton = -20;
+  double propellerCmdTethys = 0;
+  double propellerCmdDaphne = 0;
+
+  // Setup publishers and callbacks for comms topics.
+  auto senderAddressTriton = "1";
+
+  std::string receiverAddressTethys = "2";
+  std::string receiverAddressDaphne = "3";
+
+  // Set up callbacks
+  std::atomic<bool> tethysMsgReceived = false;
+  std::atomic<bool> daphneMsgReceived = false;
+
+  std::function<void(const msgs::Dataframe &)> cbTethys =
+    [&](const msgs::Dataframe &_msg)
+  {
+    tethysMsgReceived = true;
+  };
+
+  std::function<void(const msgs::Dataframe &)> cbDaphne =
+    [&](const msgs::Dataframe &_msg)
+  {
+    daphneMsgReceived = true;
+  };
+
+  node.Subscribe("/" + receiverAddressTethys + "/rx", cbTethys);
+  node.Subscribe("/" + receiverAddressDaphne + "/rx", cbDaphne);
+
+  // Publisher to send the START msg.
+  auto pub = node.Advertise<gz::msgs::Dataframe>(
+      "/broker/msgs");
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+  // Prepare and send the msgs from triton to tethys and daphne
+  msgs::Dataframe msgTethys;
+  msgs::Dataframe msgDaphne;
+
+  msgTethys.set_src_address(senderAddressTriton);
+  msgDaphne.set_src_address(senderAddressTriton);
+
+  msgTethys.set_dst_address(receiverAddressTethys);
+  msgDaphne.set_dst_address(receiverAddressDaphne);
+
+  msgTethys.set_data("START");
+  msgDaphne.set_data("START");
+
+  while (true)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+    std::cout << "--------------------------" << std::endl;
+    if (tethysMsgReceived)
+      propellerCmdTethys = -20.0;
+    else
+      pub.Publish(msgTethys);
+
+    if (daphneMsgReceived)
+      propellerCmdDaphne = -20.0;
+    else
+      pub.Publish(msgDaphne);
+
+    msgs::Double propellerMsg;
+    propellerMsg.set_data(propellerCmdTriton);
+    propellerPubTriton.Publish(propellerMsg);
+
+    propellerMsg.set_data(propellerCmdTethys);
+    propellerPubTethys.Publish(propellerMsg);
+
+    propellerMsg.set_data(propellerCmdDaphne);
+    propellerPubDaphne.Publish(propellerMsg);
+
+    std::cout << "Commanding thrust: " << std::endl;
+    std::cout << "triton : " << propellerCmdTriton << " N" << std::endl;
+    std::cout << "tethys : " << propellerCmdTethys << " N" << std::endl;
+    std::cout << "daphne : " << propellerCmdDaphne << " N" << std::endl;
+  }
+}