Multi-LRAUV Swimming Race Example (gazebosim#841)

* Multi-LRAUV swimming race example world and controller

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

* specifiy units

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

* change thruster unit to Newtons

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

* add README

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>
Signed-off-by: William Lew <WilliamMilesLew@gmail.com>

examples/standalone/multi_lrauv_race/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+
+find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log)
+set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
+
+find_package(ignition-gazebo6 REQUIRED)
+set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
+
+add_executable(multi_lrauv_race multi_lrauv_race.cc)
+target_link_libraries(multi_lrauv_race
+  ignition-transport${IGN_TRANSPORT_VER}::core
+  ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER})

examples/standalone/multi_lrauv_race/README.md

@@ -0,0 +1,25 @@
+# Multi-LRAUV Swimming Race Example
+
+This example shows the usage of the Thruster plugin and rudder joint control on
+multiple autonomous underwater vehicles (AUV) with buoyancy, lift drag, and
+hydrodynamics plugins. The multiple vehicles are differentiated by namespaces.
+
+## Build Instructions
+
+From this directory, run the following to compile:
+
+    mkdir build
+    cd build
+    cmake ..
+    make
+
+## Execute Instructions
+
+From the `build` directory, run Ignition and the example controller:
+
+    ign gazebo -r ../../../worlds/multi_lrauv_race.sdf
+    ./multi_lrauv_race
+
+The example controller will output pseudorandom propeller and rudder commands
+to move the vehicles forward. The low speed is by design to model the actual
+vehicle velocity.

examples/standalone/multi_lrauv_race/multi_lrauv_race.cc

@@ -0,0 +1,109 @@
+/*
+ * 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.
+ *
+ */
+
+/*
+ * In each iteration, for each vehicle, generate a random fin angle and thrust
+ * within reasonable limits, and send the command to the vehicle.
+ *
+ * Usage:
+ *   $ multi_lrauv_race
+ */
+
+#include <chrono>
+#include <thread>
+
+#include <ignition/msgs.hh>
+#include <ignition/transport.hh>
+
+// Fin joint limits from tethys model.sdf
+double random_angle_within_limits(double min=-0.261799, double max=0.261799)
+{
+  return min + static_cast<float>(rand()) /
+    (static_cast<float>(RAND_MAX / (max - min)));
+}
+
+// Nominal speed is thruster 300 rpm ~ 31.4 radians per second ~ 6.14 Newtons
+double random_thrust_within_limits(double min=-6.14, double max=6.14)
+{
+  return min + static_cast<float>(rand()) /
+    (static_cast<float>(RAND_MAX / (max - min)));
+}
+
+int main(int argc, char** argv)
+{
+  // Initialize random seed
+  srand(time(NULL));
+
+  std::vector<std::string> ns;
+  ns.push_back("tethys");
+  ns.push_back("triton");
+  ns.push_back("daphne");
+
+  ignition::transport::Node node;
+
+  std::vector<std::string> rudderTopics;
+  rudderTopics.resize(ns.size(), "");
+  std::vector<ignition::transport::Node::Publisher> rudderPubs;
+  rudderPubs.resize(ns.size());
+
+  std::vector<std::string> propellerTopics;
+  propellerTopics.resize(ns.size(), "");
+  std::vector<ignition::transport::Node::Publisher> propellerPubs;
+  propellerPubs.resize(ns.size());
+
+  // Set up topic names and publishers
+  for (int i = 0; i < ns.size(); i++)
+  {
+    rudderTopics[i] = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + ns[i] + "/joint/vertical_fins_joint/0/cmd_pos");
+    rudderPubs[i] = node.Advertise<ignition::msgs::Double>(rudderTopics[i]);
+
+    propellerTopics[i] = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + ns[i] + "/joint/propeller_joint/cmd_pos");
+    propellerPubs[i] = node.Advertise<ignition::msgs::Double>(
+      propellerTopics[i]);
+  }
+
+  std::vector<double> rudderCmds;
+  rudderCmds.resize(ns.size(), 0.0);
+  std::vector<double> propellerCmds;
+  propellerCmds.resize(ns.size(), 0.0);
+
+  float artificial_speedup = 1;
+
+  while (true)
+  {
+    for (int i = 0; i < ns.size(); i++)
+    {
+      rudderCmds[i] = random_angle_within_limits(-0.01, 0.01);
+      ignition::msgs::Double rudderMsg;
+      rudderMsg.set_data(rudderCmds[i]);
+      rudderPubs[i].Publish(rudderMsg);
+
+      propellerCmds[i] = random_thrust_within_limits(
+        -6.14 * artificial_speedup, 0);
+      ignition::msgs::Double propellerMsg;
+      propellerMsg.set_data(propellerCmds[i]);
+      propellerPubs[i].Publish(propellerMsg);
+
+      std::cout << "Commanding " << ns[i] << " rudder angle " << rudderCmds[i]
+        << " rad, thrust " << propellerCmds[i] << " Newtons" << std::endl;
+    }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  }
+}