From babce6a3c25c07010b22203eb75aa8530389bf4e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 26 Jan 2022 15:11:31 -0800 Subject: [PATCH] Use upstream's PointCloud GUI plugin (#139) Signed-off-by: Louise Poubel --- lrauv_ignition_plugins/CMakeLists.txt | 1 - .../src/ScienceSensorsSystem.cc | 36 +- .../src/VisualizePointCloud.cc | 499 ------------------ .../src/VisualizePointCloud.hh | 236 --------- .../src/VisualizePointCloud.qml | 233 -------- .../src/VisualizePointCloud.qrc | 5 - .../worlds/buoyant_tethys.sdf | 2 +- .../worlds/empty_environment.sdf | 91 +++- 8 files changed, 102 insertions(+), 1001 deletions(-) delete mode 100644 lrauv_ignition_plugins/src/VisualizePointCloud.cc delete mode 100644 lrauv_ignition_plugins/src/VisualizePointCloud.hh delete mode 100644 lrauv_ignition_plugins/src/VisualizePointCloud.qml delete mode 100644 lrauv_ignition_plugins/src/VisualizePointCloud.qrc diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt index 2067813f..b14ac393 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -187,7 +187,6 @@ add_lrauv_plugin(TethysCommPlugin lrauv_command lrauv_state) add_lrauv_plugin(TimeAnalysisPlugin) -add_lrauv_plugin(VisualizePointCloud GUI) add_lrauv_plugin(WorldCommPlugin PROTO lrauv_init) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index 8f226a50..56f9af32 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -43,6 +43,9 @@ using namespace tethys; class tethys::ScienceSensorsSystemPrivate { + /// \brief Advertise topics and services. + public: void StartTransport(); + ////////////////////////////////// // Functions for data manipulation @@ -1019,32 +1022,36 @@ void ScienceSensorsSystem::Configure( ignmsg << "Loading science data from [" << this->dataPtr->dataPath << "]" << std::endl; } +} +///////////////////////////////////////////////// +void ScienceSensorsSystemPrivate::StartTransport() +{ // 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->cloudPub = this->node.Advertise< + ignition::msgs::PointCloudPacked>(this->cloudTopic); - this->dataPtr->node.Advertise(this->dataPtr->cloudTopic, - &ScienceSensorsSystemPrivate::PointCloudService, this->dataPtr.get()); + this->node.Advertise(this->cloudTopic, + &ScienceSensorsSystemPrivate::PointCloudService, this); // Advertise science data, also as service and topics std::string temperatureTopic{"/temperature"}; - this->dataPtr->tempPub = this->dataPtr->node.Advertise< + this->tempPub = this->node.Advertise< ignition::msgs::Float_V>(temperatureTopic); - this->dataPtr->node.Advertise(temperatureTopic, - &ScienceSensorsSystemPrivate::TemperatureService, this->dataPtr.get()); + this->node.Advertise(temperatureTopic, + &ScienceSensorsSystemPrivate::TemperatureService, this); std::string chlorophyllTopic{"/chloropyll"}; - this->dataPtr->chlorPub = this->dataPtr->node.Advertise< + this->chlorPub = this->node.Advertise< ignition::msgs::Float_V>(chlorophyllTopic); - this->dataPtr->node.Advertise(chlorophyllTopic, - &ScienceSensorsSystemPrivate::ChlorophyllService, this->dataPtr.get()); + this->node.Advertise(chlorophyllTopic, + &ScienceSensorsSystemPrivate::ChlorophyllService, this); std::string salinityTopic{"/salinity"}; - this->dataPtr->salPub = this->dataPtr->node.Advertise< + this->salPub = this->node.Advertise< ignition::msgs::Float_V>(salinityTopic); - this->dataPtr->node.Advertise(salinityTopic, - &ScienceSensorsSystemPrivate::SalinityService, this->dataPtr.get()); + this->node.Advertise(salinityTopic, + &ScienceSensorsSystemPrivate::SalinityService, this); } ///////////////////////////////////////////////// @@ -1086,6 +1093,7 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, { this->dataPtr->sphericalCoordinatesInitialized = true; + this->dataPtr->StartTransport(); this->dataPtr->ReadData(_ecm); this->dataPtr->GenerateOctrees(); } @@ -1116,7 +1124,7 @@ void ScienceSensorsSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, } } - // Publish every n iters so that VisualizePointCloud plugin gets it. + // Publish every n iters so that GUI PointCloud plugin gets it. // Otherwise the initial publication in Configure() is not enough. if (this->dataPtr->repeatPubTimes % 10000 == 0) { diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.cc b/lrauv_ignition_plugins/src/VisualizePointCloud.cc deleted file mode 100644 index e956c9dc..00000000 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.cc +++ /dev/null @@ -1,499 +0,0 @@ -/* - * 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 "ignition/msgs/pointcloud_packed.pb.h" - -#include -#include -#include - -#include "VisualizePointCloud.hh" - -#include -#include - -#include - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -namespace tethys -{ - /// \brief Private data class for VisualizePointCloud - class VisualizePointCloudPrivate - { - /// \brief Transport node - public: ignition::transport::Node node; - - /// \brief Name of topic for PointCloudPacked - public: std::string pointCloudTopic{""}; - - /// \brief Name of topic for FloatV - public: std::string floatVTopic{""}; - - /// \brief List of topics publishing PointCloudPacked. - public: QStringList pointCloudTopicList; - - /// \brief List of topics publishing FloatV. - public: QStringList floatVTopicList; - - /// \brief Protect variables changed from transport and the user - public: std::recursive_mutex mutex; - - /// \brief Point cloud message containing location of data - public: ignition::msgs::PointCloudPacked pointCloudMsg; - - /// \brief Message holding a float vector. - public: ignition::msgs::Float_V floatVMsg; - - /// \brief Minimum value in latest float vector - public: float minFloatV{std::numeric_limits::max()}; - - /// \brief Maximum value in latest float vector - public: float maxFloatV{-std::numeric_limits::max()}; - - /// \brief Color for minimum value - public: ignition::math::Color minColor{255, 0, 0, 255}; - - /// \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}; - }; -} - -using namespace tethys; - -///////////////////////////////////////////////// -VisualizePointCloud::VisualizePointCloud() - : ignition::gui::Plugin(), - dataPtr(std::make_unique()) -{ -} - -///////////////////////////////////////////////// -VisualizePointCloud::~VisualizePointCloud() -{ - this->ClearMarkers(); -} - -///////////////////////////////////////////////// -void VisualizePointCloud::LoadConfig(const tinyxml2::XMLElement *) -{ - if (this->title.empty()) - this->title = "Visualize point cloud"; - - ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->installEventFilter(this); -} - -////////////////////////////////////////////////// -void VisualizePointCloud::OnPointCloudTopic(const QString &_pointCloudTopic) -{ - std::lock_guard(this->dataPtr->mutex); - // Unsubscribe from previous choice - if (!this->dataPtr->pointCloudTopic.empty() && - !this->dataPtr->node.Unsubscribe(this->dataPtr->pointCloudTopic)) - { - ignerr << "Unable to unsubscribe from topic [" - << this->dataPtr->pointCloudTopic <<"]" <ClearMarkers(); - - this->dataPtr->pointCloudTopic = _pointCloudTopic.toStdString(); - - // Request service - this->dataPtr->node.Request(this->dataPtr->pointCloudTopic, - &VisualizePointCloud::OnPointCloudService, this); - - // Create new subscription - if (!this->dataPtr->node.Subscribe(this->dataPtr->pointCloudTopic, - &VisualizePointCloud::OnPointCloud, this)) - { - ignerr << "Unable to subscribe to topic [" - << this->dataPtr->pointCloudTopic << "]\n"; - return; - } - ignmsg << "Subscribed to " << this->dataPtr->pointCloudTopic << std::endl; -} - -////////////////////////////////////////////////// -void VisualizePointCloud::OnFloatVTopic(const QString &_floatVTopic) -{ - std::lock_guard(this->dataPtr->mutex); - // Unsubscribe from previous choice - if (!this->dataPtr->floatVTopic.empty() && - !this->dataPtr->node.Unsubscribe(this->dataPtr->floatVTopic)) - { - ignerr << "Unable to unsubscribe from topic [" - << this->dataPtr->floatVTopic <<"]" <ClearMarkers(); - - this->dataPtr->floatVTopic = _floatVTopic.toStdString(); - - // Request service - this->dataPtr->node.Request(this->dataPtr->floatVTopic, - &VisualizePointCloud::OnFloatVService, this); - - // Create new subscription - if (!this->dataPtr->node.Subscribe(this->dataPtr->floatVTopic, - &VisualizePointCloud::OnFloatV, this)) - { - ignerr << "Unable to subscribe to topic [" - << this->dataPtr->floatVTopic << "]\n"; - return; - } - ignmsg << "Subscribed to " << this->dataPtr->floatVTopic << std::endl; -} - -////////////////////////////////////////////////// -void VisualizePointCloud::Show(bool _show) -{ - this->dataPtr->showing = _show; - if (_show) - { - this->PublishMarkers(); - } - else - { - this->ClearMarkers(); - } -} - -///////////////////////////////////////////////// -void VisualizePointCloud::OnRefresh() -{ - std::lock_guard(this->dataPtr->mutex); - ignmsg << "Refreshing topic list for point cloud messages." << std::endl; - - // Clear - this->dataPtr->pointCloudTopicList.clear(); - this->dataPtr->floatVTopicList.clear(); - - bool gotCloud = false; - - // Get updated list - std::vector allTopics; - this->dataPtr->node.TopicList(allTopics); - for (auto topic : allTopics) - { - std::vector publishers; - this->dataPtr->node.TopicInfo(topic, publishers); - for (auto pub : publishers) - { - if (pub.MsgTypeName() == "ignition.msgs.PointCloudPacked") - { - this->dataPtr->pointCloudTopicList.push_back( - QString::fromStdString(topic)); - } - else if (pub.MsgTypeName() == "ignition.msgs.Float_V") - { - this->dataPtr->floatVTopicList.push_back(QString::fromStdString(topic)); - } - } - } - // 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(); -} - -///////////////////////////////////////////////// -QStringList VisualizePointCloud::PointCloudTopicList() const -{ - return this->dataPtr->pointCloudTopicList; -} - -///////////////////////////////////////////////// -void VisualizePointCloud::SetPointCloudTopicList( - const QStringList &_pointCloudTopicList) -{ - this->dataPtr->pointCloudTopicList = _pointCloudTopicList; - this->PointCloudTopicListChanged(); -} - -///////////////////////////////////////////////// -QStringList VisualizePointCloud::FloatVTopicList() const -{ - return this->dataPtr->floatVTopicList; -} - -///////////////////////////////////////////////// -void VisualizePointCloud::SetFloatVTopicList( - const QStringList &_floatVTopicList) -{ - this->dataPtr->floatVTopicList = _floatVTopicList; - this->FloatVTopicListChanged(); -} - -////////////////////////////////////////////////// -void VisualizePointCloud::OnPointCloud( - const ignition::msgs::PointCloudPacked &_msg) -{ - std::lock_guard(this->dataPtr->mutex); - this->dataPtr->pointCloudMsg = _msg; - this->PublishMarkers(); -} - -////////////////////////////////////////////////// -void VisualizePointCloud::OnFloatV(const ignition::msgs::Float_V &_msg) -{ - std::lock_guard(this->dataPtr->mutex); - this->dataPtr->floatVMsg = _msg; - - this->dataPtr->minFloatV = std::numeric_limits::max(); - this->dataPtr->maxFloatV = -std::numeric_limits::max(); - - for (auto i = 0; i < _msg.data_size(); ++i) - { - auto data = _msg.data(i); - if (data < this->dataPtr->minFloatV) - this->SetMinFloatV(data); - if (data > this->dataPtr->maxFloatV) - 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(); -} - -////////////////////////////////////////////////// -void VisualizePointCloud::OnPointCloudService( - const ignition::msgs::PointCloudPacked &_msg, bool _result) -{ - if (!_result) - { - ignerr << "Service request failed." << std::endl; - return; - } - this->OnPointCloud(_msg); -} - -////////////////////////////////////////////////// -void VisualizePointCloud::OnFloatVService( - const ignition::msgs::Float_V &_msg, bool _result) -{ - if (!_result) - { - ignerr << "Service request failed." << std::endl; - return; - } - this->OnFloatV(_msg); -} - -////////////////////////////////////////////////// -void VisualizePointCloud::PublishMarkers() -{ - IGN_PROFILE("VisualizePointCloud::PublishMarkers"); - - if (!this->dataPtr->showing) - return; - - // If point cloud empty, do nothing. - if (this->dataPtr->pointCloudMsg.height() == 0 && - this->dataPtr->pointCloudMsg.width() == 0) - { - return; - } - - 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); - - ignition::msgs::Set(marker.mutable_scale(), - ignition::math::Vector3d::One * this->dataPtr->pointSize); - - ignition::msgs::PointCloudPackedIterator - iterX(this->dataPtr->pointCloudMsg, "x"); - ignition::msgs::PointCloudPackedIterator - iterY(this->dataPtr->pointCloudMsg, "y"); - ignition::msgs::PointCloudPackedIterator - iterZ(this->dataPtr->pointCloudMsg, "z"); - - // Index of point in point cloud, visualized or not - int ptIdx{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) - { - // 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); - } - - // Don't visualize NaN - if (std::isnan(dataVal)) - continue; - - 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 - }; - - ignition::msgs::Set(marker.add_materials()->mutable_diffuse(), color); - - ignition::msgs::Set(marker.add_point(), ignition::math::Vector3d( - *iterX, - *iterY, - *iterZ)); - } - - igndbg << "Visualizing " << marker.point_size() << " points" - << 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_id(0); - msg.set_action(ignition::msgs::Marker::DELETE_ALL); - - igndbg << "Clearing markers on " - << this->dataPtr->pointCloudTopic + this->dataPtr->floatVTopic - << std::endl; - - this->dataPtr->node.Request("/marker", msg); -} - -///////////////////////////////////////////////// -QColor VisualizePointCloud::MinColor() const -{ - return ignition::gui::convert(this->dataPtr->minColor); -} - -///////////////////////////////////////////////// -void VisualizePointCloud::SetMinColor(const QColor &_minColor) -{ - this->dataPtr->minColor = ignition::gui::convert(_minColor); - this->MinColorChanged(); - this->PublishMarkers(); -} - -///////////////////////////////////////////////// -QColor VisualizePointCloud::MaxColor() const -{ - return ignition::gui::convert(this->dataPtr->maxColor); -} - -///////////////////////////////////////////////// -void VisualizePointCloud::SetMaxColor(const QColor &_maxColor) -{ - this->dataPtr->maxColor = ignition::gui::convert(_maxColor); - this->MaxColorChanged(); - this->PublishMarkers(); -} - -///////////////////////////////////////////////// -float VisualizePointCloud::MinFloatV() const -{ - return this->dataPtr->minFloatV; -} - -///////////////////////////////////////////////// -void VisualizePointCloud::SetMinFloatV(float _minFloatV) -{ - this->dataPtr->minFloatV = _minFloatV; - this->MinFloatVChanged(); -} - -///////////////////////////////////////////////// -float VisualizePointCloud::MaxFloatV() const -{ - return this->dataPtr->maxFloatV; -} - -///////////////////////////////////////////////// -void VisualizePointCloud::SetMaxFloatV(float _maxFloatV) -{ - this->dataPtr->maxFloatV = _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 deleted file mode 100644 index 39a5d376..00000000 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.hh +++ /dev/null @@ -1,236 +0,0 @@ -/* - * 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_VISUALIZEPOINTCLOUD_HH_ -#define TETHYS_VISUALIZEPOINTCLOUD_HH_ - -#include - -#include "ignition/msgs/float_v.pb.h" -#include "ignition/msgs/pointcloud_packed.pb.h" - -#include - -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 - - /// \brief List of topics publishing PointCloudPacked messages - Q_PROPERTY( - QStringList pointCloudTopicList - READ PointCloudTopicList - WRITE SetPointCloudTopicList - NOTIFY PointCloudTopicListChanged - ) - - /// \brief List of topics publishing FloatV messages - Q_PROPERTY( - QStringList floatVTopicList - READ FloatVTopicList - WRITE SetFloatVTopicList - NOTIFY FloatVTopicListChanged - ) - - /// \brief Color for minimum value - Q_PROPERTY( - QColor minColor - READ MinColor - WRITE SetMinColor - NOTIFY MinColorChanged - ) - - /// \brief Color for maximum value - Q_PROPERTY( - QColor maxColor - READ MaxColor - WRITE SetMaxColor - NOTIFY MaxColorChanged - ) - - /// \brief Minimum value - Q_PROPERTY( - float minFloatV - READ MinFloatV - WRITE SetMinFloatV - NOTIFY MinFloatVChanged - ) - - /// \brief Maximum value - Q_PROPERTY( - float maxFloatV - READ MaxFloatV - WRITE SetMaxFloatV - NOTIFY MaxFloatVChanged - ) - - /// \brief Point size - Q_PROPERTY( - float pointSize - READ PointSize - WRITE SetPointSize - NOTIFY PointSizeChanged - ) - - /// \brief Constructor - public: VisualizePointCloud(); - - /// \brief Destructor - public: ~VisualizePointCloud() override; - - // Documentation inherited - public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; - - /// \brief Callback function for point cloud topic. - /// \param[in]_msg Point cloud message - public: void OnPointCloud(const ignition::msgs::PointCloudPacked &_msg); - - /// \brief Callback function for point cloud service - /// \param[in]_msg Point cloud message - /// \param[out]_result True on success. - public: void OnPointCloudService( - const ignition::msgs::PointCloudPacked &_msg, bool _result); - - /// \brief Get the topic list - /// \return List of topics - public: Q_INVOKABLE QStringList PointCloudTopicList() const; - - /// \brief Set the topic list from a string - /// \param[in] _pointCloudTopicList List of topics. - public: Q_INVOKABLE void SetPointCloudTopicList( - const QStringList &_pointCloudTopicList); - - /// \brief Notify that topic list has changed - signals: void PointCloudTopicListChanged(); - - /// \brief Set topic to subscribe to for point cloud. - /// \param[in] _topicName Name of selected topic - public: Q_INVOKABLE void OnPointCloudTopic(const QString &_topicName); - - /// \brief Callback function for float vector topic. - /// \param[in]_msg Float vector message - public: void OnFloatV(const ignition::msgs::Float_V &_msg); - - /// \brief Callback function for point cloud service - /// \param[in]_msg Float vector message - /// \param[out]_result True on success. - public: void OnFloatVService( - const ignition::msgs::Float_V &_msg, bool _result); - - /// \brief Get the topic list - /// \return List of topics - public: Q_INVOKABLE QStringList FloatVTopicList() const; - - /// \brief Set the topic list from a string - /// \param[in] _floatVTopicList List of topics. - public: Q_INVOKABLE void SetFloatVTopicList( - const QStringList &_floatVTopicList); - - /// \brief Notify that topic list has changed - signals: void FloatVTopicListChanged(); - - /// \brief Set topic to subscribe to for float vectors. - /// \param[in] _topicName Name of selected topic - public: Q_INVOKABLE void OnFloatVTopic(const QString &_topicName); - - /// \brief Get the minimum color - /// \return Minimum color - public: Q_INVOKABLE QColor MinColor() const; - - /// \brief Set the minimum color - /// \param[in] _minColor Minimum color. - public: Q_INVOKABLE void SetMinColor(const QColor &_minColor); - - /// \brief Notify that minimum color has changed - signals: void MinColorChanged(); - - /// \brief Get the maximum color - /// \return Maximum color - public: Q_INVOKABLE QColor MaxColor() const; - - /// \brief Set the maximum color - /// \param[ax] _maxColor Maximum color. - public: Q_INVOKABLE void SetMaxColor(const QColor &_maxColor); - - /// \brief Notify that maximum color has changed - signals: void MaxColorChanged(); - - /// \brief Get the minimum value - /// \return Minimum value - public: Q_INVOKABLE float MinFloatV() const; - - /// \brief Set the minimum value - /// \param[in] _minFloatV Minimum value. - public: Q_INVOKABLE void SetMinFloatV(float _minFloatV); - - /// \brief Notify that minimum value has changed - signals: void MinFloatVChanged(); - - /// \brief Get the maximum value - /// \return Maximum value - public: Q_INVOKABLE float MaxFloatV() const; - - /// \brief Set the maximum value - /// \param[ax] _maxFloatV Maximum value. - public: Q_INVOKABLE void SetMaxFloatV(float _maxFloatV); - - /// \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); - - /// \brief Callback when refresh button is pressed. - public: Q_INVOKABLE void OnRefresh(); - - /// \brief Makes a request to populate the scene with markers - private: void PublishMarkers(); - - /// \brief Makes a request to delete all markers related to the point cloud. - private: void ClearMarkers(); - - /// \internal - /// \brief Pointer to private data - private: std::unique_ptr dataPtr; - }; -} -#endif diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.qml b/lrauv_ignition_plugins/src/VisualizePointCloud.qml deleted file mode 100644 index 66f0e6d0..00000000 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.qml +++ /dev/null @@ -1,233 +0,0 @@ -/* - * 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" - -ColumnLayout { - spacing: 10 - Layout.minimumWidth: 350 - 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 - - Switch { - Layout.alignment: Qt.AlignHCenter - id: displayVisual - Layout.columnSpan: 5 - Layout.fillWidth: true - text: qsTr("Show") - checked: true - onToggled: { - VisualizePointCloud.Show(checked) - } - } - - RoundButton { - Layout.columnSpan: 1 - text: "\u21bb" - Material.background: Material.primary - onClicked: { - VisualizePointCloud.OnRefresh(); - pcCombo.currentIndex = 0 - floatCombo.currentIndex = 0 - } - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - ToolTip.text: qsTr("Refresh list of topics publishing point clouds and float vectors") - } - } - - GridLayout { - columns: 3 - columnSpacing: 10 - Layout.fillWidth: true - - Label { - Layout.columnSpan: 1 - text: "Point cloud" - } - - ComboBox { - Layout.columnSpan: 2 - id: pcCombo - Layout.fillWidth: true - model: VisualizePointCloud.pointCloudTopicList - currentIndex: 0 - onCurrentIndexChanged: { - if (currentIndex < 0) - return; - VisualizePointCloud.OnPointCloudTopic(textAt(currentIndex)); - } - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - ToolTip.text: qsTr("Ignition transport topics publishing PointCloudPacked messages") - } - - Label { - Layout.columnSpan: 1 - text: "Float vector" - } - - ComboBox { - Layout.columnSpan: 2 - id: floatCombo - Layout.fillWidth: true - model: VisualizePointCloud.floatVTopicList - currentIndex: 0 - onCurrentIndexChanged: { - if (currentIndex < 0) - return; - VisualizePointCloud.OnFloatVTopic(textAt(currentIndex)); - } - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - 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) - } - } - } - - RowLayout { - spacing: 10 - Layout.fillWidth: true - - Label { - Layout.columnSpan: 1 - text: isUniform() ? "Color" : "Min" - } - - Label { - Layout.columnSpan: 1 - Layout.maximumWidth: 50 - text: VisualizePointCloud.minFloatV.toFixed(4) - elide: Text.ElideRight - visible: !isUniform() - } - - Button { - Layout.columnSpan: 1 - id: minColorButton - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - ToolTip.text: qsTr("Color for minimum value") - onClicked: minColorDialog.open() - background: Rectangle { - implicitWidth: 40 - implicitHeight: 40 - radius: 5 - border.color: VisualizePointCloud.minColor - border.width: 2 - color: VisualizePointCloud.minColor - } - ColorDialog { - id: minColorDialog - title: "Choose a color for the minimum value" - visible: false - onAccepted: { - VisualizePointCloud.SetMinColor(minColorDialog.color) - minColorDialog.close() - } - onRejected: { - minColorDialog.close() - } - } - } - - Button { - Layout.columnSpan: 1 - id: maxColorButton - visible: !isUniform() - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - ToolTip.text: qsTr("Color for maximum value") - onClicked: maxColorDialog.open() - background: Rectangle { - implicitWidth: 40 - implicitHeight: 40 - radius: 5 - border.color: VisualizePointCloud.maxColor - border.width: 2 - color: VisualizePointCloud.maxColor - } - ColorDialog { - id: maxColorDialog - title: "Choose a color for the maximum value" - visible: false - onAccepted: { - VisualizePointCloud.SetMaxColor(maxColorDialog.color) - maxColorDialog.close() - } - onRejected: { - maxColorDialog.close() - } - } - } - - Label { - Layout.columnSpan: 1 - Layout.maximumWidth: 50 - text: VisualizePointCloud.maxFloatV.toFixed(4) - elide: Text.ElideRight - visible: !isUniform() - } - - Label { - Layout.columnSpan: 1 - text: "Max" - visible: !isUniform() - } - } - - Item { - Layout.columnSpan: 6 - width: 10 - Layout.fillHeight: true - } -} diff --git a/lrauv_ignition_plugins/src/VisualizePointCloud.qrc b/lrauv_ignition_plugins/src/VisualizePointCloud.qrc deleted file mode 100644 index b99f98c1..00000000 --- a/lrauv_ignition_plugins/src/VisualizePointCloud.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - VisualizePointCloud.qml - - \ No newline at end of file diff --git a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf index a467ebe0..6174a4b9 100644 --- a/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf +++ b/lrauv_ignition_plugins/worlds/buoyant_tethys.sdf @@ -265,7 +265,7 @@ docked_collapsed - + Visualize science data docked_collapsed diff --git a/lrauv_ignition_plugins/worlds/empty_environment.sdf b/lrauv_ignition_plugins/worlds/empty_environment.sdf index cea98ff8..ce76e0bb 100644 --- a/lrauv_ignition_plugins/worlds/empty_environment.sdf +++ b/lrauv_ignition_plugins/worlds/empty_environment.sdf @@ -15,6 +15,8 @@ 0.0 1.0 1.0 0.0 0.7 0.8 + + false @@ -54,13 +56,11 @@ name="tethys::TimeAnalysisPlugin"> - - + scene 0.4 0.4 0.4 0.8 0.8 0.8 + 0 6 6 0 0.5 -1.57 + + + + + + 0.1 + + 3000000 + @@ -146,6 +156,7 @@ floating false + false @@ -222,9 +233,7 @@ Plot Tethys 3D path - floating_collapsed - 0 - 0 + docked_collapsed tethys 0 0 1 @@ -234,17 +243,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 + @@ -261,6 +310,24 @@ -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + 300000 300000 + + + 1.0 + + + +