diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf
index d5debb0f..b00d65ac 100644
--- a/lrauv_description/models/tethys_equipped/model.sdf
+++ b/lrauv_description/models/tethys_equipped/model.sdf
@@ -118,8 +118,10 @@
0.17
0
0.0244
+
0 1 0
- 1 0 0
+
+ -1 0 0
vertical_fins
0 0 0
@@ -135,8 +137,10 @@
0.17
0
0.0244
+
0 0 1
- 1 0 0
+
+ -1 0 0
horizontal_fins
0 0 0
diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt
index 7fa15115..0f74232c 100644
--- a/lrauv_ignition_plugins/CMakeLists.txt
+++ b/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
diff --git a/lrauv_ignition_plugins/example/example_elevator.cc b/lrauv_ignition_plugins/example/example_elevator.cc
new file mode 100644
index 00000000..5fe35f3a
--- /dev/null
+++ b/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
+ */
+#include
+#include
+
+#include
+#include
+#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(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;
+}
diff --git a/lrauv_ignition_plugins/proto/lrauv_command.proto b/lrauv_ignition_plugins/proto/lrauv_command.proto
index ac6195c8..ba2281ac 100644
--- a/lrauv_ignition_plugins/proto/lrauv_command.proto
+++ b/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
diff --git a/lrauv_ignition_plugins/test/test_elevator.cc b/lrauv_ignition_plugins/test/test_elevator.cc
new file mode 100644
index 00000000..998a5510
--- /dev/null
+++ b/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
+#include
+
+#include "helper/LrauvTestFixture.hh"
+#include "lrauv_command.pb.h"
+
+#include
+
+//////////////////////////////////////////////////
+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();
+ }
+}
+