diff --git a/lrauv_ignition_plugins/data/simple_test.csv b/lrauv_ignition_plugins/data/simple_test.csv index 7449b802..175950a0 100644 --- a/lrauv_ignition_plugins/data/simple_test.csv +++ b/lrauv_ignition_plugins/data/simple_test.csv @@ -1,31 +1,31 @@ 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.0000001,0.0000000,0,-5.0,0.001,NaN,-1,0.5 -0,0.0000002,0.0000000,0,-5.1,0.002,NaN,-1,1.5 -0,0.0000003,0.0000000,0,-5.2,0.001,NaN,-1,0.5 -0,0.0000004,0.0000000,0,-5.3,0.002,0.1,-1,0.5 -0,0.0000005,0.0000000,0,-5.4,0.001,0.1,-1,0.5 -0,0.0000001,0.0000001,0,-5.5,0.002,0.1,-1,0.5 -0,0.0000002,0.0000001,0,-5.6,0.001,0.1,-1,0.5 -0,0.0000003,0.0000001,0,-5.7,0.002,0.1,-1,0.5 -0,0.0000004,0.0000001,0,-5.8,0.001,0.1,-1,1.5 -0,0.0000005,0.0000001,0,-5.9,0.002,0.1,-1,0.5 -0,0.0000001,0.0000002,0,-6.0,0.001,0.1,-1,0.5 -0,0.0000002,0.0000002,0,-6.1,0.002,NaN,-1,0.5 -0,0.0000003,0.0000002,0,-6.2,0.001,NaN,-1,0.5 -0,0.0000004,0.0000002,0,-6.3,0.002,NaN,-1,0.5 -0,0.0000005,0.0000002,0,-6.4,0.001,NaN,-1,1.5 -0,0.0000001,0.0000003,10,-6.5,0.002,NaN,-1,0.5 -0,0.0000002,0.0000003,10,-6.6,0.001,NaN,-1,0.5 -0,0.0000003,0.0000003,10,-6.7,0.002,NaN,-1,0.5 -0,0.0000004,0.0000003,10,-6.8,0.001,NaN,-1,0.5 -0,0.0000005,0.0000003,10,-6.9,0.002,NaN,-1,0.5 -0,0.0000001,0.0000004,10,-7.0,0.001,NaN,-1,0.5 -0,0.0000002,0.0000004,10,-7.1,0.002,0.1,-1,0.5 -0,0.0000003,0.0000004,10,-7.2,0.001,0.1,-1,1.5 -0,0.0000004,0.0000004,10,-7.3,0.002,0.1,-1,0.5 -0,0.0000005,0.0000004,10,-7.4,0.001,0.1,-1,0.5 -0,0.0000001,0.0000005,10,-7.5,0.002,0.1,-1,0.5 -0,0.0000002,0.0000005,10,-7.6,0.001,NaN,-1,0.5 -0,0.0000003,0.0000005,10,-7.7,0.002,NaN,-1,1.5 -0,0.0000004,0.0000005,10,-7.8,0.001,NaN,-1,0.5 -0,0.0000005,0.0000005,10,-7.9,0.002,NaN,-1,0.5 +0,0.000001,0.000000,0,-5.0,0.001,NaN,-1,0.5 +0,0.000002,0.000000,0,-5.1,0.002,NaN,-1,1.5 +0,0.000003,0.000000,0,-5.2,0.001,NaN,-1,0.5 +0,0.000004,0.000000,0,-5.3,0.002,0.1,-1,0.5 +0,0.000005,0.000000,0,-5.4,0.001,0.1,-1,0.5 +0,0.000001,0.000001,0,-5.5,0.002,0.1,-1,0.5 +0,0.000002,0.000001,0,-5.6,0.001,0.1,-1,0.5 +0,0.000003,0.000001,0,-5.7,0.002,0.1,-1,0.5 +0,0.000004,0.000001,0,-5.8,0.001,0.1,-1,1.5 +0,0.000005,0.000001,0,-5.9,0.002,0.1,-1,0.5 +0,0.000001,0.000002,0,-6.0,0.001,0.1,-1,0.5 +0,0.000002,0.000002,0,-6.1,0.002,NaN,-1,0.5 +0,0.000003,0.000002,0,-6.2,0.001,NaN,-1,0.5 +0,0.000004,0.000002,0,-6.3,0.002,NaN,-1,0.5 +0,0.000005,0.000002,0,-6.4,0.001,NaN,-1,1.5 +0,0.000001,0.000003,0.5,-6.5,0.002,NaN,-1,0.5 +0,0.000002,0.000003,0.5,-6.6,0.001,NaN,-1,0.5 +0,0.000003,0.000003,0.5,-6.7,0.002,NaN,-1,0.5 +0,0.000004,0.000003,0.5,-6.8,0.001,NaN,-1,0.5 +0,0.000005,0.000003,0.5,-6.9,0.002,NaN,-1,0.5 +0,0.000001,0.000004,0.5,-7.0,0.001,NaN,-1,0.5 +0,0.000002,0.000004,0.5,-7.1,0.002,0.1,-1,0.5 +0,0.000003,0.000004,0.5,-7.2,0.001,0.1,-1,1.5 +0,0.000004,0.000004,0.5,-7.3,0.002,0.1,-1,0.5 +0,0.000005,0.000004,0.5,-7.4,0.001,0.1,-1,0.5 +0,0.000001,0.000005,0.5,-7.5,0.002,0.1,-1,0.5 +0,0.000002,0.000005,0.5,-7.6,0.001,NaN,-1,0.5 +0,0.000003,0.000005,0.5,-7.7,0.002,NaN,-1,1.5 +0,0.000004,0.000005,0.5,-7.8,0.001,NaN,-1,0.5 +0,0.000005,0.000005,0.5,-7.9,0.002,NaN,-1,0.5 diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index f81b831f..f49b6038 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.000001; - // 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( @@ -855,11 +821,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..0c38f183 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(); } @@ -347,21 +346,15 @@ void VisualizePointCloud::PublishMarkers() return; } - 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; + 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); PointCloudPackedIterator iterX(this->dataPtr->pointCloudMsg, "x"); PointCloudPackedIterator iterY(this->dataPtr->pointCloudMsg, "y"); @@ -369,21 +362,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 = 1.0; if (this->dataPtr->floatVMsg.data().size() > ptIdx) { dataVal = this->dataPtr->floatVMsg.data(ptIdx); @@ -393,76 +379,32 @@ 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); - + // TODO(chapulina) color per-point + // https://github.com/osrf/lrauv/issues/52 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); } ////////////////////////////////////////////////// @@ -535,6 +477,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..778b27eb 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -100,6 +100,10 @@ 0.4 0.4 0.4 0.8 0.8 0.8 0 6 6 0 0.5 -1.57 + + 0.25 + 100000 + @@ -238,9 +242,7 @@ Plot Tethys 3D path - floating_collapsed - 0 - 0 + docked_collapsed tethys 0 0 1 @@ -250,15 +252,19 @@ Inspector - floating_collapsed - 400 + docked_collapsed Visualize science data - floating_collapsed - 800 + docked_collapsed + + + + + Camera controls + docked_collapsed