Multi-LRAUV Swimming Race Example

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/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));
+  }
+}