From 4f7259af1b5d954565f4ef91d296dd5674ad52c8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 23 Nov 2021 13:58:29 -0800 Subject: [PATCH] Fix thruster coefficient Signed-off-by: Louise Poubel --- .../models/tethys_equipped/model.sdf | 2 +- lrauv_ignition_plugins/CMakeLists.txt | 4 +- .../src/TethysCommPlugin.cc | 2 +- .../test/helper/LrauvTestFixture.hh | 14 +++++ .../test/test_controller.cc | 6 +- .../test/test_mission_yoyo_circle.cc | 4 +- lrauv_ignition_plugins/test/test_state_msg.cc | 60 +++++++++++++++++++ 7 files changed, 83 insertions(+), 9 deletions(-) create mode 100644 lrauv_ignition_plugins/test/test_state_msg.cc diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index f0081a8e..156ac74a 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -94,7 +94,7 @@ tethys propeller_joint - 0.00004422 + 0.004422 1000 0.2 true diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt index e5d23784..e3c5ca01 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -243,7 +243,8 @@ if(BUILD_TESTING) test_mission_pitch_mass test_mission_yoyo_circle test_rudder - test_spawn) + test_spawn + test_state_msg) add_executable( ${TEST_TARGET} @@ -256,6 +257,7 @@ if(BUILD_TESTING) PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} lrauv_command lrauv_init + lrauv_state ) include_directories(${CMAKE_CURRENT_BINARY_DIR}) gtest_discover_tests(${TEST_TARGET}) diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index 46abb956..0d8c4d59 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -422,7 +422,7 @@ void TethysCommPlugin::CommandCallback( // force = thrust_coefficient * fluid_density * omega ^ 2 * // propeller_diameter ^ 4 // These values are defined in the model's Thruster plugin's SDF - auto force = 0.00004422 * 1000 * 0.2 * angVel * angVel; + auto force = 0.004422 * 1000 * pow(0.2, 4) * angVel * angVel; if (angVel < 0) { force *=-1; diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index 02012949..76dd0f30 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -32,6 +32,7 @@ #include #include "lrauv_command.pb.h" +#include "lrauv_state.pb.h" #include "TestConstants.hh" @@ -62,6 +63,9 @@ class LrauvTestFixture : public ::testing::Test this->node.Advertise( commandTopic); + auto stateTopic = "/tethys/state_topic"; + this->node.Subscribe(stateTopic, &LrauvTestFixture::OnState, this); + // Setup fixture this->fixture = std::make_unique( ignition::common::joinPaths( @@ -106,6 +110,13 @@ class LrauvTestFixture : public ::testing::Test EXPECT_LT(sleep, maxSleep); } + /// Callback function for state from TethysComm + /// \param[in] _msg State message + private: void OnState(const lrauv_ignition_plugins::msgs::LRAUVState &_msg) + { + this->stateMsgs.push_back(_msg); + } + /// \brief Check that a pose is within a given range. /// \param[in] _index Pose index in tethysPoses /// \param[in] _refPose Reference pose @@ -241,6 +252,9 @@ class LrauvTestFixture : public ::testing::Test /// \brief All tethys world poses in order public: std::vector tethysPoses; + /// \brief All state messages in order + public: std::vector stateMsgs; + /// \brief Test fixture public: std::unique_ptr fixture{nullptr}; diff --git a/lrauv_ignition_plugins/test/test_controller.cc b/lrauv_ignition_plugins/test/test_controller.cc index 7ccef9af..b47c211d 100644 --- a/lrauv_ignition_plugins/test/test_controller.cc +++ b/lrauv_ignition_plugins/test/test_controller.cc @@ -71,7 +71,7 @@ TEST_F(LrauvTestFixture, Command) double dtSec = std::chrono::duration(this->dt).count(); ASSERT_LT(0.0, dtSec); double time100it = 100 * dtSec; - for (unsigned int i = 1000; i < this->tethysPoses.size(); i += 100) + for (unsigned int i = 1800; i < this->tethysPoses.size(); i += 100) { auto prevPose = this->tethysPoses[i - 100]; auto pose = this->tethysPoses[i]; @@ -80,9 +80,7 @@ TEST_F(LrauvTestFixture, Command) auto linVel = dist / time100it; EXPECT_LT(0.0, linVel); - - // TODO(chapulina) Decrease tolerance - EXPECT_NEAR(1.0, linVel, 0.19) << i; + EXPECT_NEAR(1.0, linVel, 0.06) << i; } } diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 84783479..0bd13814 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -97,10 +97,10 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Depth is above 20m, and below 2m after initial descent, with some // tolerance - EXPECT_LT(-22.1, pose.Pos().Z()) << i; + EXPECT_LT(-22.4, pose.Pos().Z()) << i; if (i > 2000) { - EXPECT_GT(0.0, pose.Pos().Z()) << i; + EXPECT_GT(0.23, pose.Pos().Z()) << i; } // Pitch is between -20 and 20 deg diff --git a/lrauv_ignition_plugins/test/test_state_msg.cc b/lrauv_ignition_plugins/test/test_state_msg.cc new file mode 100644 index 00000000..1e968fb3 --- /dev/null +++ b/lrauv_ignition_plugins/test/test_state_msg.cc @@ -0,0 +1,60 @@ +/* + * 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. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +#include +#include + +#include "helper/LrauvTestFixture.hh" +#include "lrauv_command.pb.h" +#include "lrauv_state.pb.h" + +////////////////////////////////////////////////// +TEST_F(LrauvTestFixture, Command) +{ + // TODO(chapulina) Test other fields, see + // https://github.com/osrf/lrauv/pull/81 + + // Initial state + this->fixture->Server()->Run(true, 100, false); + EXPECT_EQ(100, this->stateMsgs.size()); + + auto latest = this->stateMsgs.back(); + EXPECT_NEAR(0.0, latest.propomega_(), 1e-6); + + // Propel vehicle forward by giving the propeller a positive angular velocity + lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg; + cmdMsg.set_propomegaaction_(10 * IGN_PI); + + // Neutral buoyancy + cmdMsg.set_buoyancyaction_(0.0005); + cmdMsg.set_dropweightstate_(true); + + // Run server until we collect more states + this->PublishCommandWhile(cmdMsg, [&]() + { + return this->stateMsgs.size() < 2000; + }); + + latest = this->stateMsgs.back(); + EXPECT_NEAR(10.0 * IGN_PI, latest.propomega_(), 1e-6); +} +