diff --git a/examples/standalone/point_cloud/CMakeLists.txt b/examples/standalone/point_cloud/CMakeLists.txt new file mode 100644 index 000000000..668511bfb --- /dev/null +++ b/examples/standalone/point_cloud/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(ignition-gui-point-cloud-example) + +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + find_package(ignition-transport12 QUIET REQUIRED OPTIONAL_COMPONENTS log) + set(IGN_TRANSPORT_VER ${ignition-transport12_VERSION_MAJOR}) + + find_package(ignition-common5 REQUIRED) + set(IGN_COMMON_VER ${ignition-common5_VERSION_MAJOR}) + + find_package(ignition-msgs9 REQUIRED) + set(IGN_MSGS_VER ${ignition-msgs9_VERSION_MAJOR}) + + add_executable(point_cloud point_cloud.cc) + target_link_libraries(point_cloud + ignition-transport${IGN_TRANSPORT_VER}::core + ignition-msgs${IGN_MSGS_VER} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ) +endif() diff --git a/examples/standalone/point_cloud/README.md b/examples/standalone/point_cloud/README.md new file mode 100644 index 000000000..b0c5c0b8b --- /dev/null +++ b/examples/standalone/point_cloud/README.md @@ -0,0 +1,43 @@ +# Point cloud visualization example + +This example publishes a point cloud that can be visualized with the +`PointCloud` plugin. The example publishes: + +* `ignition::msgs::PointCloudPacked` messages on `/point_cloud` with XYZ + positions of points. +* `ignition::msgs::FloatV` messages on 3 different topics: + * `/flat`: Array of 1s + * `/sum`: Each value in the array corresponds to the sum of its point's + coordinates (X + Y + Z). + * `/product`: Each value in the array corresponds to the product of its + point's coordinates (X * Y * Z). + +## Build Instructions + +Navigate to this directory: + + cd /examples/standalone/point_cloud + +Build: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +1. Navigate to this directory: + + cd /examples/standalone/point_cloud + +1. Launch the example config file: + + ign gui -c point_cloud.config + +3. From the build directory above, publish clouds to be visualized: + + ./point_cloud + +4. Try out different options on the plugin. + diff --git a/examples/standalone/point_cloud/point_cloud.cc b/examples/standalone/point_cloud/point_cloud.cc new file mode 100644 index 000000000..b2d036839 --- /dev/null +++ b/examples/standalone/point_cloud/point_cloud.cc @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#include +#include + +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +// Set to true if a signal has been received to terminate publishing +static std::atomic g_terminatePub(false); + +///////////////////////////////////////////////// +int main(int _argc, char **_argv) +{ + // Install a signal handler for SIGINT and SIGTERM. + auto signalHandler = [](int _signal) -> void + { + if (_signal == SIGINT || _signal == SIGTERM) + g_terminatePub = true; + }; + std::signal(SIGINT, signalHandler); + std::signal(SIGTERM, signalHandler); + + // Create messages + ignition::msgs::PointCloudPacked pcMsg; + ignition::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, + {{"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32}}); + + int numberOfPoints{1000}; + unsigned int dataSize{numberOfPoints * pcMsg.point_step()}; + pcMsg.mutable_data()->resize(dataSize); + pcMsg.set_height(1); + pcMsg.set_width(1000); + + ignition::msgs::Float_V flatMsg; + ignition::msgs::Float_V sumMsg; + ignition::msgs::Float_V productMsg; + + // Populate messages + ignition::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); + ignition::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + ignition::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (float x = 0.0, y = 0.0, z = 0.0; + xIter != xIter.End(); + ++xIter, ++yIter, ++zIter) + { + *xIter = x; + *yIter = y; + *zIter = z; + flatMsg.add_data(1); + sumMsg.add_data(x + y + z); + productMsg.add_data(x * y * z); + + x += 1.0; + if (x > 9) + { + x = 0.0; + y += 1.0; + } + if (y > 9) + { + y = 0.0; + z += 1.0; + } + } + + // Publish messages at 1Hz until interrupted. + ignition::transport::Node node; + auto flatPub = node.Advertise("/flat"); + auto sumPub = node.Advertise("/sum"); + auto productPub = node.Advertise("/product"); + auto pcPub = node.Advertise("/point_cloud"); + + while (!g_terminatePub) + { + std::cout << "Publishing" << std::endl; + flatPub.Publish(flatMsg); + sumPub.Publish(sumMsg); + productPub.Publish(productMsg); + pcPub.Publish(pcMsg); + std::this_thread::sleep_for(1s); + } +} diff --git a/examples/standalone/point_cloud/point_cloud.config b/examples/standalone/point_cloud/point_cloud.config new file mode 100644 index 000000000..951710cf3 --- /dev/null +++ b/examples/standalone/point_cloud/point_cloud.config @@ -0,0 +1,63 @@ + + + + + View 1 + docked + + ogre2 + scene + 1 1 1 + 0.8 0.8 0.8 + -10 5 10 0 0.5 0 + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + Flat + docked + + /point_cloud + /flat + + + + Sum + docked + + /point_cloud + /sum + + + + Product + docked + + /point_cloud + /product + diff --git a/include/ignition/gui/Helpers.hh b/include/ignition/gui/Helpers.hh index b99b1822d..7ca3efe09 100644 --- a/include/ignition/gui/Helpers.hh +++ b/include/ignition/gui/Helpers.hh @@ -108,8 +108,14 @@ namespace ignition T findFirstByProperty(const QList _list, const char *_key, QVariant _value) { + if (nullptr == _key) + return nullptr; + for (const auto &w : _list) { + if (nullptr == w) + continue; + if (w->property(_key) == _value) return w; } diff --git a/src/Helpers_TEST.cc b/src/Helpers_TEST.cc index 3a3479703..f1185a5c8 100644 --- a/src/Helpers_TEST.cc +++ b/src/Helpers_TEST.cc @@ -126,12 +126,15 @@ TEST(HelpersTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(findFirstByProperty)) // Construct a list auto o0 = new QObject(); + ASSERT_NE(nullptr, o0); o0->setProperty("banana", 1.0); auto o1 = new QObject(); + ASSERT_NE(nullptr, o1); o1->setProperty("banana", 2.0); auto o2 = new QObject(); + ASSERT_NE(nullptr, o2); o2->setProperty("banana", 1.0); QList list; diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 7666bb759..74356ed29 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -120,6 +120,7 @@ add_subdirectory(image_display) add_subdirectory(interactive_view_control) add_subdirectory(key_publisher) add_subdirectory(plotting) +add_subdirectory(point_cloud) add_subdirectory(publisher) add_subdirectory(marker_manager) add_subdirectory(minimal_scene) diff --git a/src/plugins/point_cloud/CMakeLists.txt b/src/plugins/point_cloud/CMakeLists.txt new file mode 100644 index 000000000..0f9f2119a --- /dev/null +++ b/src/plugins/point_cloud/CMakeLists.txt @@ -0,0 +1,10 @@ +ign_gui_add_plugin(PointCloud + SOURCES + PointCloud.cc + QT_HEADERS + PointCloud.hh + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + TEST_SOURCES + PointCloud_TEST.cc +) diff --git a/src/plugins/point_cloud/PointCloud.cc b/src/plugins/point_cloud/PointCloud.cc new file mode 100644 index 000000000..77ccbbc3c --- /dev/null +++ b/src/plugins/point_cloud/PointCloud.cc @@ -0,0 +1,515 @@ +/* + * Copyright (C) 2022 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. + * + */ + +#include "ignition/msgs/pointcloud_packed.pb.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "PointCloud.hh" + +/// \brief Private data class for PointCloud +class ignition::gui::plugins::PointCloudPrivate +{ + /// \brief Makes a request to populate the scene with markers + public: void PublishMarkers(); + + /// \brief Makes a request to delete all markers related to the point cloud. + public: void ClearMarkers(); + + /// \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 XYZ positions + 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, changeable at runtime + public: ignition::math::Color minColor{1.0f, 0.0f, 0.0f, 1.0f}; + + /// \brief Color for maximum value, changeable at runtime + public: ignition::math::Color maxColor{0.0f, 1.0f, 0.0f, 1.0f}; + + /// \brief Size of each point, changeable at runtime + public: float pointSize{20}; + + /// \brief True if showing, changeable at runtime + public: bool showing{true}; +}; + +using namespace ignition; +using namespace gui; +using namespace plugins; + +///////////////////////////////////////////////// +PointCloud::PointCloud() + : ignition::gui::Plugin(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +PointCloud::~PointCloud() +{ + this->dataPtr->ClearMarkers(); +} + +///////////////////////////////////////////////// +void PointCloud::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Point cloud"; + + // Parameters from XML + if (_pluginElem) + { + auto pointCloudTopicElem = + _pluginElem->FirstChildElement("point_cloud_topic"); + if (nullptr != pointCloudTopicElem && + nullptr != pointCloudTopicElem->GetText()) + { + this->SetPointCloudTopicList({pointCloudTopicElem->GetText()}); + this->OnPointCloudTopic(this->dataPtr->pointCloudTopicList.at(0)); + } + + auto floatVTopicElem = + _pluginElem->FirstChildElement("float_v_topic"); + if (nullptr != floatVTopicElem && + nullptr != floatVTopicElem->GetText()) + { + this->SetFloatVTopicList({floatVTopicElem->GetText()}); + this->OnFloatVTopic(this->dataPtr->floatVTopicList.at(0)); + } + + } + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +////////////////////////////////////////////////// +void PointCloud::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 <<"]" <dataPtr->ClearMarkers(); + + this->dataPtr->pointCloudTopic = _pointCloudTopic.toStdString(); + + // Request service + this->dataPtr->node.Request(this->dataPtr->pointCloudTopic, + &PointCloud::OnPointCloudService, this); + + // Create new subscription + if (!this->dataPtr->node.Subscribe(this->dataPtr->pointCloudTopic, + &PointCloud::OnPointCloud, this)) + { + ignerr << "Unable to subscribe to topic [" + << this->dataPtr->pointCloudTopic << "]\n"; + return; + } + ignmsg << "Subscribed to " << this->dataPtr->pointCloudTopic << std::endl; +} + +////////////////////////////////////////////////// +void PointCloud::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 <<"]" <dataPtr->ClearMarkers(); + + this->dataPtr->floatVTopic = _floatVTopic.toStdString(); + + // Request service + this->dataPtr->node.Request(this->dataPtr->floatVTopic, + &PointCloud::OnFloatVService, this); + + // Create new subscription + if (!this->dataPtr->node.Subscribe(this->dataPtr->floatVTopic, + &PointCloud::OnFloatV, this)) + { + ignerr << "Unable to subscribe to topic [" + << this->dataPtr->floatVTopic << "]\n"; + return; + } + ignmsg << "Subscribed to " << this->dataPtr->floatVTopic << std::endl; +} + +////////////////////////////////////////////////// +void PointCloud::Show(bool _show) +{ + this->dataPtr->showing = _show; + if (_show) + { + this->dataPtr->PublishMarkers(); + } + else + { + this->dataPtr->ClearMarkers(); + } +} + +///////////////////////////////////////////////// +void PointCloud::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(); + + // 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 PointCloud::PointCloudTopicList() const +{ + return this->dataPtr->pointCloudTopicList; +} + +///////////////////////////////////////////////// +void PointCloud::SetPointCloudTopicList( + const QStringList &_pointCloudTopicList) +{ + this->dataPtr->pointCloudTopicList = _pointCloudTopicList; + this->PointCloudTopicListChanged(); +} + +///////////////////////////////////////////////// +QStringList PointCloud::FloatVTopicList() const +{ + return this->dataPtr->floatVTopicList; +} + +///////////////////////////////////////////////// +void PointCloud::SetFloatVTopicList( + const QStringList &_floatVTopicList) +{ + this->dataPtr->floatVTopicList = _floatVTopicList; + this->FloatVTopicListChanged(); +} + +////////////////////////////////////////////////// +void PointCloud::OnPointCloud( + const ignition::msgs::PointCloudPacked &_msg) +{ + std::lock_guard(this->dataPtr->mutex); + this->dataPtr->pointCloudMsg = _msg; + this->dataPtr->PublishMarkers(); +} + +////////////////////////////////////////////////// +void PointCloud::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->dataPtr->PublishMarkers(); +} + +////////////////////////////////////////////////// +void PointCloud::OnPointCloudService( + const ignition::msgs::PointCloudPacked &_msg, bool _result) +{ + if (!_result) + { + ignerr << "Service request failed." << std::endl; + return; + } + this->OnPointCloud(_msg); +} + +////////////////////////////////////////////////// +void PointCloud::OnFloatVService( + const ignition::msgs::Float_V &_msg, bool _result) +{ + if (!_result) + { + ignerr << "Service request failed." << std::endl; + return; + } + this->OnFloatV(_msg); +} + +////////////////////////////////////////////////// +void PointCloudPrivate::PublishMarkers() +{ + IGN_PROFILE("PointCloud::PublishMarkers"); + + if (!this->showing) + return; + + // If point cloud empty, do nothing. + if (this->pointCloudMsg.height() == 0 && + this->pointCloudMsg.width() == 0) + { + return; + } + + std::lock_guard(this->mutex); + ignition::msgs::Marker marker; + marker.set_ns(this->pointCloudTopic + this->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->pointSize); + + ignition::msgs::PointCloudPackedIterator + iterX(this->pointCloudMsg, "x"); + ignition::msgs::PointCloudPackedIterator + iterY(this->pointCloudMsg, "y"); + ignition::msgs::PointCloudPackedIterator + iterZ(this->pointCloudMsg, "z"); + + // Index of point in point cloud, visualized or not + int ptIdx{0}; + auto minC = this->minColor; + auto maxC = this->maxColor; + auto floatRange = this->maxFloatV - this->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->floatVMsg.data().size() > ptIdx) + { + dataVal = this->floatVMsg.data(ptIdx); + } + + // Don't visualize NaN + if (std::isnan(dataVal)) + continue; + + auto ratio = floatRange > 0 ? + (dataVal - this->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)); + } + + this->node.Request("/marker", marker); +} + +////////////////////////////////////////////////// +void PointCloudPrivate::ClearMarkers() +{ + if (this->pointCloudTopic.empty()) + return; + + std::lock_guard(this->mutex); + ignition::msgs::Marker msg; + msg.set_ns(this->pointCloudTopic + this->floatVTopic); + msg.set_id(0); + msg.set_action(ignition::msgs::Marker::DELETE_ALL); + + igndbg << "Clearing markers on " + << this->pointCloudTopic + this->floatVTopic + << std::endl; + + this->node.Request("/marker", msg); +} + +///////////////////////////////////////////////// +QColor PointCloud::MinColor() const +{ + return ignition::gui::convert(this->dataPtr->minColor); +} + +///////////////////////////////////////////////// +void PointCloud::SetMinColor(const QColor &_minColor) +{ + this->dataPtr->minColor = ignition::gui::convert(_minColor); + this->MinColorChanged(); + this->dataPtr->PublishMarkers(); +} + +///////////////////////////////////////////////// +QColor PointCloud::MaxColor() const +{ + return ignition::gui::convert(this->dataPtr->maxColor); +} + +///////////////////////////////////////////////// +void PointCloud::SetMaxColor(const QColor &_maxColor) +{ + this->dataPtr->maxColor = ignition::gui::convert(_maxColor); + this->MaxColorChanged(); + this->dataPtr->PublishMarkers(); +} + +///////////////////////////////////////////////// +float PointCloud::MinFloatV() const +{ + return this->dataPtr->minFloatV; +} + +///////////////////////////////////////////////// +void PointCloud::SetMinFloatV(float _minFloatV) +{ + this->dataPtr->minFloatV = _minFloatV; + this->MinFloatVChanged(); +} + +///////////////////////////////////////////////// +float PointCloud::MaxFloatV() const +{ + return this->dataPtr->maxFloatV; +} + +///////////////////////////////////////////////// +void PointCloud::SetMaxFloatV(float _maxFloatV) +{ + this->dataPtr->maxFloatV = _maxFloatV; + this->MaxFloatVChanged(); +} + +///////////////////////////////////////////////// +float PointCloud::PointSize() const +{ + return this->dataPtr->pointSize; +} + +///////////////////////////////////////////////// +void PointCloud::SetPointSize(float _pointSize) +{ + this->dataPtr->pointSize = _pointSize; + this->PointSizeChanged(); + this->dataPtr->PublishMarkers(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gui::plugins::PointCloud, + ignition::gui::Plugin) diff --git a/src/plugins/point_cloud/PointCloud.hh b/src/plugins/point_cloud/PointCloud.hh new file mode 100644 index 000000000..feb56edd8 --- /dev/null +++ b/src/plugins/point_cloud/PointCloud.hh @@ -0,0 +1,250 @@ +/* + * Copyright (C) 2022 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. + * + */ + +#ifndef IGNITION_GUI_PLUGINS_POINTCLOUD_HH_ +#define IGNITION_GUI_PLUGINS_POINTCLOUD_HH_ + +#include + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "ignition/gui/Plugin.hh" + +namespace ignition +{ +namespace gui +{ +namespace plugins +{ + class PointCloudPrivate; + + /// \brief Visualize `ignition::msgs::PointCloudPacked` messages in a 3D + /// scene. + /// + /// By default, the whole cloud is displayed using a single color. Users + /// can optionally choose a topic publishing `ignition::msgs::FloatV` messages + /// which will be used to color all points with a color gradient according to + /// their values. The float message must have the same number of elements as + /// the point cloud and be indexed the same way. NaN values on the FloatV + /// message aren't displayed. + /// + /// Requirements: + /// * A plugin that loads a 3D scene, such as `MinimalScene` + /// * The `MarkerManager` plugin + /// + /// Parameters: + /// + /// * ``: Topic to receive + /// `ignition::msgs::PointCloudPacked` messages. + /// * ``: Topic to receive `ignition::msgs::FloatV` messages. + class PointCloud : 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: PointCloud(); + + /// \brief Destructor + public: ~PointCloud() 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 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 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 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 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[in] _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[in] _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[in] _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(); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/plugins/point_cloud/PointCloud.qml b/src/plugins/point_cloud/PointCloud.qml new file mode 100644 index 000000000..5a4ef1eab --- /dev/null +++ b/src/plugins/point_cloud/PointCloud.qml @@ -0,0 +1,229 @@ +/* + * Copyright (C) 2022 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. + * + */ + +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 ignition.gui 1.0 +import "qrc:/qml" + +ColumnLayout { + spacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 350 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + function isUniform() { + return PointCloud.minFloatV >= PointCloud.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: { + PointCloud.Show(checked) + } + } + + RoundButton { + Layout.columnSpan: 1 + text: "\u21bb" + Material.background: Material.primary + onClicked: { + PointCloud.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: PointCloud.pointCloudTopicList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + PointCloud.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: PointCloud.floatVTopicList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + PointCloud.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: PointCloud.pointSize + minimumValue: 1 + maximumValue: 1000 + decimals: 0 + onEditingFinished: { + PointCloud.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: PointCloud.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: PointCloud.minColor + border.width: 2 + color: PointCloud.minColor + } + ColorDialog { + id: minColorDialog + title: "Choose a color for the minimum value" + visible: false + onAccepted: { + PointCloud.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: PointCloud.maxColor + border.width: 2 + color: PointCloud.maxColor + } + ColorDialog { + id: maxColorDialog + title: "Choose a color for the maximum value" + visible: false + onAccepted: { + PointCloud.SetMaxColor(maxColorDialog.color) + maxColorDialog.close() + } + onRejected: { + maxColorDialog.close() + } + } + } + + Label { + Layout.columnSpan: 1 + Layout.maximumWidth: 50 + text: PointCloud.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/src/plugins/point_cloud/PointCloud.qrc b/src/plugins/point_cloud/PointCloud.qrc new file mode 100644 index 000000000..321ddf2ef --- /dev/null +++ b/src/plugins/point_cloud/PointCloud.qrc @@ -0,0 +1,5 @@ + + + PointCloud.qml + + diff --git a/src/plugins/point_cloud/PointCloud_TEST.cc b/src/plugins/point_cloud/PointCloud_TEST.cc new file mode 100644 index 000000000..fee3dd1ec --- /dev/null +++ b/src/plugins/point_cloud/PointCloud_TEST.cc @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#include +#include + +#include +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "ignition/gui/Application.hh" +#include "ignition/gui/MainWindow.hh" +#include "ignition/gui/Plugin.hh" +#include "PointCloud.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("./PointCloud_TEST")), +}; + +using namespace ignition; +using namespace gui; + +///////////////////////////////////////////////// +TEST(PointCloudTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PointCloud)) +{ + common::Console::SetVerbosity(4); + + // Load the plugin + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + EXPECT_TRUE(app.LoadPlugin("PointCloud")); + + // Get main window + auto window = app.findChild(); + ASSERT_NE(window, nullptr); + + // Get plugin + auto plugins = window->findChildren(); + EXPECT_EQ(plugins.size(), 1); + + // Cleanup + plugins.clear(); +}