Elevator example, test and docs

lrauv_description/models/tethys_equipped/model.sdf

@@ -118,8 +118,10 @@
         <alpha_stall>0.17</alpha_stall>
         <a0>0</a0>
         <area>0.0244</area>
+        <!-- Link's Y is perpendicular to the control surface -->
         <upward>0 1 0</upward>
-        <forward>1 0 0</forward>
+        <!-- Link's X is pointing towards the back -->
+        <forward>-1 0 0</forward>
         <link_name>vertical_fins</link_name>
         <cp>0 0 0</cp>
       </plugin>
@@ -135,8 +137,10 @@
         <alpha_stall>0.17</alpha_stall>
         <a0>0</a0>
         <area>0.0244</area>
+        <!-- Link's Z is perpendicular to the control surface -->
         <upward>0 0 1</upward>
-        <forward>1 0 0</forward>
+        <!-- Link's X is pointing towards the back -->
+        <forward>-1 0 0</forward>
         <link_name>horizontal_fins</link_name>
         <cp>0 0 0</cp>
       </plugin>

lrauv_ignition_plugins/CMakeLists.txt

@@ -178,6 +178,7 @@ install(
 foreach(EXAMPLE
   example_buoyancy
   example_controller
+  example_elevator
   example_mass_shifter
   example_rudder
   example_spawn
@@ -237,6 +238,7 @@ if(BUILD_TESTING)
     test_buoyancy_action
     test_controller
     test_drop_weight
+    test_elevator
     test_mass_shifter
     test_mission_depth_vbs
     test_mission_pitch_mass

lrauv_ignition_plugins/example/example_elevator.cc

@@ -0,0 +1,70 @@
+/*
+ * 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.
+ *
+ */
+
+/**
+ * Control the elevator joint position.
+ *
+ * Positive angles rotate the horizontal fins counter-clockwise when looking
+ * from starboard, which makes the vehicle go down when propelled forward.
+ * Negative commands do the opposite.
+ *
+ * Usage:
+ *   $ LRAUV_example_elevator <vehicle_name> <angle_radians>
+ */
+#include <chrono>
+#include <thread>
+
+#include <ignition/msgs.hh>
+#include <ignition/transport.hh>
+#include "lrauv_command.pb.h"
+
+int main(int _argc, char **_argv)
+{
+  std::string vehicleName("tethys");
+  if (_argc > 1)
+  {
+    vehicleName = _argv[1];
+  }
+
+  double angle{0.17};
+  if (_argc > 2)
+  {
+    angle = atof(_argv[2]);
+  }
+
+  ignition::transport::Node node;
+  auto commandTopic = "/" + vehicleName + "/command_topic";
+  auto commandPub =
+    node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);
+
+  while (!commandPub.HasConnections())
+  {
+    std::cout << "Command publisher waiting for connections..." << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  }
+
+  lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
+  cmdMsg.set_elevatorangleaction_(angle);
+
+  // Keep it stable
+  cmdMsg.set_buoyancyaction_(0.0005);
+  cmdMsg.set_dropweightstate_(1);
+
+  commandPub.Publish(cmdMsg);
+
+  std::cout << "Moving elevator to [" << angle << "] rad" << std::endl;
+}

lrauv_ignition_plugins/proto/lrauv_command.proto

@@ -43,7 +43,12 @@ message LRAUVCommand
                                      // Higher values have the vertical fins
                                      // rotated more clockwise when looking
                                      // from the top (i.e. to starboard)
-  float elevatorAngle_         = 4;
+  float elevatorAngle_         = 4;  // Angle that the controller believes the
+                                     // elevator is currently at. Unit: radians.
+                                     // Higher values have the horizontal fins
+                                     // rotated more counter-clockwise when
+                                     // looking from starboard, which tilts the
+                                     // nose downward when moving forward.
   float massPosition_          = 5;  // Position where the controller believes
                                      // the mass shifter's joint is in. Unit:
                                      // meters. Positive values have the battery
@@ -60,7 +65,11 @@ message LRAUVCommand
                                      // radians. Higher values rotate the
                                      // vertical fins clockwise when looking
                                      // from the top (i.e. to starboard)
-  float elevatorAngleAction_   = 10;
+  float elevatorAngleAction_   = 10;  // Target angle for the elevator joint.
+                                      // Unit: radians. Higher values rotate
+                                      // vertical fins more counter-clockwise
+                                      // when looking from starboard, which tilts
+                                      // the nose downward when moving forward.
   float massPositionAction_    = 11;  // Target position for the battery's joint.
                                       // Unit: meters. Positive values move the
                                       // battery forward, tilting the nose

lrauv_ignition_plugins/test/test_elevator.cc

@@ -0,0 +1,77 @@
+/*
+ * 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.
+ *
+ */
+
+#include <chrono>
+#include <gtest/gtest.h>
+
+#include "helper/LrauvTestFixture.hh"
+#include "lrauv_command.pb.h"
+
+#include <fstream>
+
+//////////////////////////////////////////////////
+TEST_F(LrauvTestFixture, Elevator)
+{
+  this->fixture->Server()->Run(true, 100, false);
+  EXPECT_EQ(100, this->iterations);
+  EXPECT_EQ(100, this->tethysPoses.size());
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1e-6);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Z(), 2e-2);
+
+  // Propel vehicle
+  lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
+
+  // Move forward
+  cmdMsg.set_propomegaaction_(30);
+
+  // Rotate elevator counter-clockwise when looking from starboard, which
+  // causes the vehicle to pitch down
+  // Using an angle lower than the stall angle 0.17
+  cmdMsg.set_elevatorangleaction_(0.15);
+
+  // Neutral buoyancy
+  cmdMsg.set_buoyancyaction_(0.0005);
+  cmdMsg.set_dropweightstate_(true);
+
+  // Run server until the command is processed and the model reaches a certain
+  // point vertically
+  double targetZ{-1.5};
+  this->PublishCommandWhile(cmdMsg, [&]()
+  {
+    return this->tethysPoses.back().Pos().Z() > targetZ;
+  });
+
+  EXPECT_LT(1500, this->iterations);
+  EXPECT_LT(1500, this->tethysPoses.size());
+
+  // Vehicle goes down
+  EXPECT_GT(targetZ, this->tethysPoses.back().Pos().Z());
+
+  for (int i = 0; i < this->tethysPoses.size(); ++i)
+  {
+    auto pose = this->tethysPoses[i];
+
+    // FIXME(chapulina) It goes up a little bit in the beginning, is this
+    // expected?
+    if (i < 1500)
+      EXPECT_GT(0.2, pose.Pos().Z()) << i << " -- " << pose.Pos().Z();
+    else
+      EXPECT_GT(0.0, pose.Pos().Z()) << i << " -- " << pose.Pos().Z();
+  }
+}
+