From 1b1a44ca67ab82c8d42425df5089c23be55b3139 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 6 Dec 2021 14:48:44 -0800 Subject: [PATCH 01/17] Migrate to Ignition Garden Signed-off-by: Louise Poubel --- README.md | 2 +- docker/empty_world/Dockerfile | 4 ++-- docker/tests/Dockerfile | 2 +- .../models/tethys_equipped/model.sdf | 2 +- lrauv_ignition_plugins/CMakeLists.txt | 16 ++++++++-------- lrauv_ignition_plugins/src/WorldCommPlugin.cc | 2 +- lrauv_ignition_plugins/worlds/multi_lrauv.sdf | 4 ++-- lrauv_ignition_plugins/worlds/star_world.sdf | 8 ++++---- 8 files changed, 20 insertions(+), 20 deletions(-) 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/empty_world/Dockerfile b/docker/empty_world/Dockerfile index 853a0297..ec23b9f3 100644 --- a/docker/empty_world/Dockerfile +++ b/docker/empty_world/Dockerfile @@ -73,8 +73,8 @@ 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://raw.githubusercontent.com/ignition-tooling/gazebodistro/master/collection-fortress.yaml \ - && vcs import < collection-fortress.yaml + && wget https://raw.githubusercontent.com/ignition-tooling/gazebodistro/master/collection-garden.yaml \ + && vcs import < collection-garden.yaml # Install Ignition dependencies # This parses Ignition source tree to find package dependencies diff --git a/docker/tests/Dockerfile b/docker/tests/Dockerfile index c3f3b9c2..beaf9803 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -45,7 +45,7 @@ RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable # 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..11ad3b71 100644 --- a/lrauv_description/models/tethys_equipped/model.sdf +++ b/lrauv_description/models/tethys_equipped/model.sdf @@ -180,7 +180,7 @@ + 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..4f324809 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -20,17 +20,17 @@ 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-sensors7 REQUIRED) +set(IGN_SENSORS_VER ${ignition-sensors7_VERSION_MAJOR}) -find_package(ignition-msgs8 REQUIRED) -set(IGN_MSGS_VER ${ignition-msgs8_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}) 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/worlds/multi_lrauv.sdf b/lrauv_ignition_plugins/worlds/multi_lrauv.sdf index 7beed595..4b06618d 100644 --- a/lrauv_ignition_plugins/worlds/multi_lrauv.sdf +++ b/lrauv_ignition_plugins/worlds/multi_lrauv.sdf @@ -131,7 +131,7 @@ triton/command_topic triton/state_topic - + triton @@ -174,7 +174,7 @@ daphne/command_topic daphne/state_topic - + daphne diff --git a/lrauv_ignition_plugins/worlds/star_world.sdf b/lrauv_ignition_plugins/worlds/star_world.sdf index 8652a38a..bb6b6f17 100644 --- a/lrauv_ignition_plugins/worlds/star_world.sdf +++ b/lrauv_ignition_plugins/worlds/star_world.sdf @@ -96,7 +96,7 @@ tethys2/command_topic tethys2/state_topic - + tethys2 @@ -137,7 +137,7 @@ tethys3/command_topic tethys3/state_topic - + tethys3 @@ -178,7 +178,7 @@ tethys4/command_topic tethys4/state_topic - + tethys4 @@ -196,6 +196,6 @@ - + From 1b1c856f03c04cd2e7190360ee879e04987d0660 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 6 Dec 2021 14:54:19 -0800 Subject: [PATCH 02/17] nightlies Signed-off-by: Louise Poubel --- docker/debug_integration/Dockerfile | 3 ++- docker/empty_world/Dockerfile | 1 + docker/tests/Dockerfile | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/docker/debug_integration/Dockerfile b/docker/debug_integration/Dockerfile index ee30fcb6..3f3f6a09 100644 --- a/docker/debug_integration/Dockerfile +++ b/docker/debug_integration/Dockerfile @@ -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' && \ +RUN /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 ec23b9f3..beb101b3 100644 --- a/docker/empty_world/Dockerfile +++ b/docker/empty_world/Dockerfile @@ -79,6 +79,7 @@ RUN mkdir -p ${IGN_WS}/src \ # 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' \ +RUN /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 beaf9803..6d720b24 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -41,6 +41,7 @@ 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' && \ +RUN /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 From 4e110eaa936203b546af075880f5b60082eb198e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 19 Nov 2021 21:46:24 -0800 Subject: [PATCH 03/17] Remove all performance tricks, use points, increase camera clip distance, grid improvements Signed-off-by: Louise Poubel --- .../src/ScienceSensorsSystem.cc | 39 ----- .../src/VisualizePointCloud.cc | 145 ++++++------------ .../src/VisualizePointCloud.hh | 19 +++ .../src/VisualizePointCloud.qml | 20 ++- .../worlds/buoyant_tethys.sdf | 36 ++++- 5 files changed, 115 insertions(+), 144 deletions(-) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 392f6e25..da38c188 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -190,23 +190,6 @@ class tethys::ScienceSensorsSystemPrivate /// \brief Publish a few more times for visualization plugin to get them public: int repeatPubTimes = 1; - - // 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; }; ///////////////////////////////////////////////// @@ -445,12 +428,6 @@ 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}, @@ -458,17 +435,6 @@ void ScienceSensorsSystemPrivate::ReadData( ignition::math::SphericalCoordinates::LOCAL2); 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. this->timeSpaceCoords[lineTimeIdx]->push_back( @@ -856,11 +822,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/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc index fad96d2f..90386aba 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}; }; @@ -304,6 +299,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 +347,12 @@ void VisualizePointCloud::PublishMarkers() } std::lock_guard(this->dataPtr->mutex); - - // 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::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); PointCloudPackedIterator iterX(this->dataPtr->pointCloudMsg, "x"); PointCloudPackedIterator iterY(this->dataPtr->pointCloudMsg, "y"); @@ -369,21 +360,14 @@ 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; 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 + float dataVal = 0.0; if (this->dataPtr->floatVMsg.data().size() > ptIdx) { dataVal = this->dataPtr->floatVMsg.data(ptIdx); @@ -393,89 +377,46 @@ 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 = (dataVal - this->dataPtr->minFloatV) / (this->dataPtr->maxFloatV - this->dataPtr->minFloatV); - auto color = this->dataPtr->minColor + - (this->dataPtr->maxColor - this->dataPtr->minColor) * ratio; + 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 + }; - 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); + ignition::msgs::Set(marker.mutable_material()->mutable_diffuse(), color); + ignition::msgs::Set(marker.add_materials()->mutable_diffuse(), color); - // 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(marker.mutable_scale(), + ignition::math::Vector3d::One * this->dataPtr->pointSize); - 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 +476,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..d99533fa 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.hh @@ -87,6 +87,14 @@ namespace tethys NOTIFY MaxFloatVChanged ) + /// \brief Point size + Q_PROPERTY( + float pointSize + READ PointSize + WRITE SetPointSize + NOTIFY PointSizeChanged + ) + /// \brief Constructor public: VisualizePointCloud(); @@ -192,6 +200,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..66d05f0a 100644 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.qml +++ b/lrauv_ignition_plugins/src/VisualizePointCloud.qml @@ -30,7 +30,7 @@ import "qrc:/qml" ColumnLayout { spacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 300 + Layout.minimumHeight: 350 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -112,6 +112,22 @@ ColumnLayout { ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval ToolTip.text: qsTr("Ignition transport topics publishing FloatV messages") } + + Label { + Layout.columnSpan: 1 + text: "Point size" + } + + IgnSpinBox { + id: pointSizeSpin + value: VisualizePointCloud.pointSize + minimumValue: 1 + maximumValue: 1000 + decimals: 0 + onEditingFinished: { + VisualizePointCloud.SetPointSize(pointSizeSpin.value) + } + } } RowLayout { @@ -133,7 +149,6 @@ ColumnLayout { 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 +178,6 @@ ColumnLayout { Button { Layout.columnSpan: 1 id: maxColorButton - Layout.fillWidth: true ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval ToolTip.text: qsTr("Color for maximum value") diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 7e73ea61..7ef89960 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 @@ -100,6 +102,12 @@ 0.4 0.4 0.4 0.8 0.8 0.8 0 6 6 0 0.5 -1.57 + + + 0.001 + + 300000 + @@ -238,9 +246,7 @@ Plot Tethys 3D path - floating_collapsed - 0 - 0 + docked_collapsed tethys 0 0 1 @@ -250,16 +256,32 @@ 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 From 36eea70c0006a555b0faf3809962b7e5101e73b8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Dec 2021 17:51:28 -0800 Subject: [PATCH 04/17] grids Signed-off-by: Louise Poubel --- .../worlds/buoyant_tethys.sdf | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 7ef89960..55031b58 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -101,12 +101,13 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - 0 6 6 0 0.5 -1.57 + + 0 0 800000 3.14 1.5 3.1 0.001 - - 300000 + + 3000000 @@ -275,13 +276,24 @@ docked_collapsed - - 6 - 0 - - 50000 - 0 100000 0 0 0 0.32 - 0 1 0 1 + + + 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 + From ac8141ac4863674eab619fbb166f6271a29e0b8c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Dec 2021 20:04:41 -0800 Subject: [PATCH 05/17] fix Dockerfiles Signed-off-by: Louise Poubel --- docker/debug_integration/Dockerfile | 2 +- docker/empty_world/Dockerfile | 2 +- docker/tests/Dockerfile | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docker/debug_integration/Dockerfile b/docker/debug_integration/Dockerfile index 3f3f6a09..b5a141ad 100644 --- a/docker/debug_integration/Dockerfile +++ b/docker/debug_integration/Dockerfile @@ -41,7 +41,7 @@ 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' && \ -RUN /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 '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 diff --git a/docker/empty_world/Dockerfile b/docker/empty_world/Dockerfile index beb101b3..e18a5c41 100644 --- a/docker/empty_world/Dockerfile +++ b/docker/empty_world/Dockerfile @@ -79,7 +79,7 @@ RUN mkdir -p ${IGN_WS}/src \ # 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' \ -RUN /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 '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 6d720b24..db828c74 100644 --- a/docker/tests/Dockerfile +++ b/docker/tests/Dockerfile @@ -41,7 +41,7 @@ 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' && \ -RUN /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 '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 From 61528526dfa8417324eee6c1bd84056d93642e47 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Dec 2021 21:46:33 -0800 Subject: [PATCH 06/17] clip Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/worlds/buoyant_tethys.sdf | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 55031b58..4167b410 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -101,11 +101,14 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 + - 0 0 800000 3.14 1.5 3.1 + + -50000 -30000 250000 0 1.1 1.58 - 0.001 + + 0.1 3000000 From 0db3884e71e8ebb489e23e3c3c68a616140cb9be Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 8 Dec 2021 12:26:22 -0800 Subject: [PATCH 07/17] add plane Signed-off-by: Louise Poubel --- .../worlds/buoyant_tethys.sdf | 23 ++++++------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 4167b410..7f8af77f 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -314,32 +314,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 - --> + - + 0 6 6 0 0.5 -1.57 - -50000 -30000 250000 0 1.1 1.58 + From 704a289a3f62ca233b844a6538da1d883f1d7d8c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 21 Dec 2021 13:40:25 -0800 Subject: [PATCH 13/17] Grid: modify default grid and add GUI plugin Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/worlds/buoyant_tethys.sdf | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 787b40fa..fdd64b08 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 @@ -258,6 +260,20 @@ docked_collapsed + + + docked_collapsed + + + + 100 + 0 + + 1 + 0 0 0 0 0 0 + 0.5 0.5 0.5 1 + + Tethys controls From c77869ce28b478bea3a8b002404f017650718bf7 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 21 Dec 2021 21:20:17 -0800 Subject: [PATCH 14/17] Fix test segfaults - but why are they happening on this branch?! Signed-off-by: Louise Poubel --- .../src/ScienceSensorsSystem.cc | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 2367bb22..638d8224 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -869,7 +869,8 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( } SortIndices(xsSorted, xsSortedInds); - int ltPSortedIdx, gtPSortedIdx; + int ltPSortedIdx{-1}; + int gtPSortedIdx{-1}; float ltPDist, gtPDist; // Get the two closest positions in _xs that _p lies between. @@ -888,6 +889,15 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( break; } } + if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() || + gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size()) + { + ignwarn << "Failed to find consecutive elements in sorted vector. Indices [" + << ltPSortedIdx << "] / [" << gtPSortedIdx + << "] for vector sized [" << xsSortedInds.size() + << "]. Cannot interpolate." << 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); @@ -896,6 +906,12 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( // Retrieve indices of sorted elements in original array int ltPIdx = xsSortedInds[ltPSortedIdx]; int gtPIdx = xsSortedInds[gtPSortedIdx]; + + if (ltPIdx >= _values.size() || gtPIdx >= _values.size()) + { + ignwarn << "Bad indices. Cannot interpolate." << std::endl; + return std::numeric_limits::quiet_NaN(); + } float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx]; if (this->DEBUG_INTERPOLATE) @@ -938,7 +954,7 @@ void ScienceSensorsSystemPrivate::SortIndices( // 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 + // elements of equal values std::stable_sort(_idx.begin(), _idx.end(), [&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];}); } From 1e6697da3cb1b0e13417e0105fb1e944cdad3d1f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 21 Dec 2021 21:28:45 -0800 Subject: [PATCH 15/17] Remove miniature scale from science data and fix segfault Signed-off-by: Louise Poubel --- .../src/ScienceSensorsSystem.cc | 42 +++++++--------- .../worlds/buoyant_tethys.sdf | 48 ++++++++++++------- 2 files changed, 50 insertions(+), 40 deletions(-) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index da85aeb0..a3cfff44 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -296,18 +296,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. @@ -567,11 +555,6 @@ void ScienceSensorsSystemPrivate::ReadData( // 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) { @@ -906,7 +889,8 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( } SortIndices(xsSorted, xsSortedInds); - int ltPSortedIdx, gtPSortedIdx; + int ltPSortedIdx{-1}; + int gtPSortedIdx{-1}; float ltPDist, gtPDist; // Get the two closest positions in _xs that _p lies between. @@ -925,6 +909,15 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( break; } } + if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() || + gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size()) + { + ignwarn << "Failed to find consecutive elements in sorted vector. Indices [" + << ltPSortedIdx << "] / [" << gtPSortedIdx + << "] for vector sized [" << xsSortedInds.size() + << "]. Cannot interpolate." << 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); @@ -933,6 +926,12 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( // Retrieve indices of sorted elements in original array int ltPIdx = xsSortedInds[ltPSortedIdx]; int gtPIdx = xsSortedInds[gtPSortedIdx]; + + if (ltPIdx >= _values.size() || gtPIdx >= _values.size()) + { + ignwarn << "Bad indices. Cannot interpolate." << std::endl; + return std::numeric_limits::quiet_NaN(); + } float result = ltPWeight * _values[ltPIdx] + gtPWeight * _values[gtPIdx]; if (this->DEBUG_INTERPOLATE) @@ -975,7 +974,7 @@ void ScienceSensorsSystemPrivate::SortIndices( // 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 + // elements of equal values std::stable_sort(_idx.begin(), _idx.end(), [&_v](size_t _i1, size_t _i2) {return _v[_i1] < _v[_i2];}); } @@ -1396,11 +1395,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/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index fdd64b08..6af05bea 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -101,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 + @@ -260,10 +270,25 @@ docked_collapsed + + + Camera controls + docked_collapsed + + docked_collapsed + + + 6 + 0 + + 50000 + 0 100000 0 0 0 0.32 + 0 1 0 1 + 100 @@ -303,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 - --> + - 35.5999984741211 - -121.779998779297 - + 0 + 0 0 0 @@ -82,9 +80,8 @@ - 2003080103_mb_l3_las.csv - + simple_test.csv From 4d79b2a2887b066553cc48a371192bf68644e8ea Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 20:40:22 -0800 Subject: [PATCH 17/17] cleanup bad merge and revert data removal Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/src/ScienceSensorsSystem.cc | 9 --------- lrauv_ignition_plugins/worlds/buoyant_tethys.sdf | 9 ++++++--- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index b7bd3903..8f226a50 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -894,15 +894,6 @@ float ScienceSensorsSystemPrivate::BarycentricInterpolate( break; } } - if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() || - gtPSortedIdx < 0 || gtPSortedIdx >= xsSortedInds.size()) - { - ignwarn << "Failed to find consecutive elements in sorted vector. Indices [" - << ltPSortedIdx << "] / [" << gtPSortedIdx - << "] for vector sized [" << xsSortedInds.size() - << "]. Cannot interpolate." << std::endl; - return std::numeric_limits::quiet_NaN(); - } // Sanity check if (ltPSortedIdx < 0 || ltPSortedIdx >= xsSortedInds.size() || diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index 8073baf6..6af05bea 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -65,14 +65,16 @@ ENU + 35.5999984741211 + -121.779998779297 - 0 - 0 + 0 0 @@ -80,8 +82,9 @@ + 2003080103_mb_l3_las.csv - simple_test.csv +