From db9b7f4cb17a7aec556b5b2b37f403489de588bd Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Thu, 20 Aug 2020 18:48:43 +0530 Subject: [PATCH 1/7] visualize lidar plugin Signed-off-by: Mihir Kulkarni --- examples/worlds/visualize_lidar.sdf | 487 ++++++++++++++++++ src/gui/plugins/CMakeLists.txt | 1 + .../plugins/visualize_lidar/CMakeLists.txt | 6 + .../plugins/visualize_lidar/VisualizeLidar.cc | 445 ++++++++++++++++ .../plugins/visualize_lidar/VisualizeLidar.hh | 132 +++++ .../visualize_lidar/VisualizeLidar.qml | 121 +++++ .../visualize_lidar/VisualizeLidar.qrc | 5 + 7 files changed, 1197 insertions(+) create mode 100644 examples/worlds/visualize_lidar.sdf create mode 100644 src/gui/plugins/visualize_lidar/CMakeLists.txt create mode 100644 src/gui/plugins/visualize_lidar/VisualizeLidar.cc create mode 100644 src/gui/plugins/visualize_lidar/VisualizeLidar.hh create mode 100644 src/gui/plugins/visualize_lidar/VisualizeLidar.qml create mode 100644 src/gui/plugins/visualize_lidar/VisualizeLidar.qrc diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf new file mode 100644 index 0000000000..325178eb59 --- /dev/null +++ b/examples/worlds/visualize_lidar.sdf @@ -0,0 +1,487 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/visualize_lidar_world/control + /world/visualize_lidar_world/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/visualize_lidar_world/stats + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + + 20 20 0.1 + + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 4 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + lidar + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + + true + + + + -4 0 0.325 0 0 0.0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0 0 0.5 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 0 0 0 0 0 0 + lidar2 + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 1 + 1 + 0.0 + 0.0 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + lidar_link + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + + + + + + 0 0 0 0 0 1.57 + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Playground + + + + + diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index c2ef4a81b0..99e4caafc1 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -90,3 +90,4 @@ add_subdirectory(shapes) add_subdirectory(transform_control) add_subdirectory(video_recorder) add_subdirectory(view_angle) +add_subdirectory(visualize_lidar) diff --git a/src/gui/plugins/visualize_lidar/CMakeLists.txt b/src/gui/plugins/visualize_lidar/CMakeLists.txt new file mode 100644 index 0000000000..3806d4ad62 --- /dev/null +++ b/src/gui/plugins/visualize_lidar/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(VisualizeLidar + SOURCES VisualizeLidar.cc + QT_HEADERS VisualizeLidar.hh + PRIVATE_LINK_LIBS + ${IGNITION-RENDERING_LIBRARIES} +) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc new file mode 100644 index 0000000000..58df59f2bf --- /dev/null +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -0,0 +1,445 @@ +/* + * Copyright (C) 2020 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 +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +#include "ignition/rendering/RenderTypes.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/Scene.hh" +#include "ignition/rendering/LidarVisual.hh" + +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Util.hh" + +#include "ignition/msgs/laserscan.pb.h" +#include "VisualizeLidar.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + /// \brief Private data class for VisualizeLidar + class VisualizeLidarPrivate + { + /// \brief Transport node + public: transport::Node node; + + /// \brief Scene Pointer + public: rendering::ScenePtr scene; + + /// \brief Pointer to LidarVisual + public: rendering::LidarVisualPtr lidar; + + /// \brief Visual type for lidar visual + public: rendering::LidarVisualType visualType; + + /// \brief URI sequence to the lidar link + public: std::string lidarString{""}; + + /// \brief LaserScan message from sensor + public: msgs::LaserScan msg; + + /// \brief Pose of the lidar visual + public: math::Pose3d lidarPose{math::Pose3d::Zero}; + + /// \brief Current state of the checkbox + public: bool checkboxState{false}; + + /// \brief Topic name to subscribe + public: std::string topicName; + + /// \brief Entity representing the sensor in the world + public: gazebo::Entity lidarEntity; + + /// \brief Minimum range for the visual + public: double minVisualRange; + + /// \brief Maximum range for the visual + public: double maxVisualRange; + + /// \brief Mutex for variable mutated by the checkbox and spinboxes + /// callbacks. + /// The variables are: msg, visualType, minVisualRange and + /// maxVisualRange + public: std::mutex serviceMutex; + + /// \brief Initialization flag + public: bool initialized{false}; + + /// \brief lidar visual display dirty flag + public: bool visualDirty{false}; + + /// \brief lidar sensor entity dirty flag + public: bool lidarEntityDirty{false}; + + /// \brief Name of the world + public: std::string worldName; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +VisualizeLidar::VisualizeLidar() + : GuiSystem(), dataPtr(new VisualizeLidarPrivate) +{ + // no ops +} + +///////////////////////////////////////////////// +VisualizeLidar::~VisualizeLidar() = default; + +///////////////////////////////////////////////// +void VisualizeLidar::LoadLidar() +{ + auto loadedEngNames = rendering::loadedEngines(); + if (loadedEngNames.empty()) + return; + + // assume there is only one engine loaded + auto engineName = loadedEngNames[0]; + if (loadedEngNames.size() > 1) + { + igndbg << "More than one engine is available. " + << "VisualizeLidar plugin will use engine [" + << engineName << "]" << std::endl; + } + auto engine = rendering::engine(engineName); + if (!engine) + { + ignerr << "Internal error: failed to load engine [" << engineName + << "]. VisualizeLidar plugin won't work." << std::endl; + return; + } + + if (engine->SceneCount() == 0) + return; + + // assume there is only one scene + // load scene + auto scene = engine->SceneByIndex(0); + if (!scene) + { + ignerr << "Internal error: scene is null." << std::endl; + return; + } + + if (!scene->IsInitialized() || scene->VisualCount() == 0) + { + return; + } + + // Create lidar visual + igndbg << "Creating lidar visual" << std::endl; + + auto root = scene->RootVisual(); + this->dataPtr->lidar = scene->CreateLidarVisual(); + if (!this->dataPtr->lidar) + { + ignwarn << "Failed to create lidar, lidar visual plugin won't work." + << std::endl; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->removeEventFilter(this); + return; + } + if (this->dataPtr->lidar) + { + root->AddChild(this->dataPtr->lidar); + this->dataPtr->initialized = true; + } +} + +///////////////////////////////////////////////// +void VisualizeLidar::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Visualize lidar"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool VisualizeLidar::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gazebo::gui::events::Render::kType) + { + // This event is called in Scene3d's RenderThread, so it's safe to make + // rendering calls here + + std::lock_guard lock(this->dataPtr->serviceMutex); + if (!this->dataPtr->initialized) + { + this->LoadLidar(); + } + + if (this->dataPtr->lidar) + { + if (this->dataPtr->visualDirty) + { + this->dataPtr->lidar->SetWorldPose(this->dataPtr->lidarPose); + this->dataPtr->lidar->Update(); + this->dataPtr->visualDirty = false; + } + } + else + { + ignerr << "Lidar pointer is not set" << std::endl; + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +////////////////////////////////////////////////// +void VisualizeLidar::Update(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("VisualizeLidar::Update"); + + std::lock_guard lock(this->dataPtr->serviceMutex); + + if (this->dataPtr->lidarEntityDirty) + { + auto lidarURIVec = common::split(common::trimmed( + this->dataPtr->lidarString), "::"); + if (lidarURIVec.size() > 0) + { + auto baseEntity = _ecm.EntityByComponents( + components::Name(lidarURIVec[0])); + if (!baseEntity) + { + ignerr << "Error entity " << lidarURIVec[0] + << " doesn't exist and cannot be used to set lidar visual pose" + << std::endl; + return; + } + else + { + auto parent = baseEntity; + bool success = false; + for (auto i = 0ul; i < lidarURIVec.size()-1; i++) + { + auto children = _ecm.EntitiesByComponents( + components::ParentEntity(parent)); + bool foundChild = false; + for (auto child : children) + { + std::string nextstring = lidarURIVec[i+1]; + std::string childname = std::string( + _ecm.Component(child)->Data()); + if (nextstring.compare(childname) == 0) + { + parent = child; + foundChild = true; + if (i+1 == lidarURIVec.size()-1) + { + success = true; + } + break; + } + } + if (foundChild == false) + { + ignerr << "The entity could not be found." + << "Error displaying lidar visual" <dataPtr->lidarEntity = parent; + this->dataPtr->lidarEntityDirty = false; + } + } + } + } + if (this->dataPtr->lidarEntity) + { + this->dataPtr->lidarPose = worldPose(this->dataPtr->lidarEntity, _ecm); + } +} + +////////////////////////////////////////////////// +void VisualizeLidar::UpdateType(int _type) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + switch (_type) { + case 0: this->dataPtr->visualType = + rendering::LidarVisualType::LVT_NONE; + break; + case 1: this->dataPtr->visualType = + rendering::LidarVisualType::LVT_RAY_LINES; + break; + case 2: this->dataPtr->visualType = + rendering::LidarVisualType::LVT_POINTS; + break; + case 3: this->dataPtr->visualType = + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS; + break; + default: this->dataPtr->visualType = + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS; + break; + } + this->dataPtr->lidar->SetType(this->dataPtr->visualType); +} + +////////////////////////////////////////////////// +void VisualizeLidar::SetTopicName(const QString &_topicName) +{ + std::lock_guard(this->dataPtr->serviceMutex); + this->dataPtr->node.Unsubscribe(this->dataPtr->topicName); + this->dataPtr->topicName = _topicName.toStdString(); + + // Reset visualization + this->ResetLidarVisual(); + + // Create new subscription + if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName, + &VisualizeLidar::OnScan, this)) + { + ignerr << "Input subscriber could not be created for topic [" + << this->dataPtr->topicName << "]\n"; + return; + } + ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl; + this->TopicNameChanged(); +} + +////////////////////////////////////////////////// +QString VisualizeLidar::TopicName() const +{ + return QString::fromStdString(this->dataPtr->topicName); +} + +////////////////////////////////////////////////// +void VisualizeLidar::UpdateNonHitting(bool _value) +{ + std::lock_guard(this->dataPtr->serviceMutex); + this->dataPtr->lidar->SetDisplayNonHitting(_value); +} + +////////////////////////////////////////////////// +void VisualizeLidar::OnScan(const msgs::LaserScan &_msg) +{ + std::lock_guard(this->dataPtr->serviceMutex); + if (this->dataPtr->initialized) + { + this->dataPtr->msg = std::move(_msg); + this->dataPtr->lidar->SetVerticalRayCount( + this->dataPtr->msg.vertical_count()); + this->dataPtr->lidar->SetHorizontalRayCount( + this->dataPtr->msg.count()); + this->dataPtr->lidar->SetMinHorizontalAngle( + this->dataPtr->msg.angle_min()); + this->dataPtr->lidar->SetMaxHorizontalAngle( + this->dataPtr->msg.angle_max()); + this->dataPtr->lidar->SetMinVerticalAngle( + this->dataPtr->msg.vertical_angle_min()); + this->dataPtr->lidar->SetMaxVerticalAngle( + this->dataPtr->msg.vertical_angle_max()); + + this->dataPtr->lidar->SetPoints(std::vector( + this->dataPtr->msg.ranges().begin(), + this->dataPtr->msg.ranges().end())); + + this->dataPtr->visualDirty = true; + + for (auto data_values : this->dataPtr->msg.header().data()) + { + if (data_values.key() == "frame_id") + { + if (this->dataPtr->lidarString.compare( + common::trimmed(data_values.value(0))) != 0) + { + this->dataPtr->lidarString = common::trimmed(data_values.value(0)); + this->dataPtr->lidarEntityDirty = true; + this->dataPtr->maxVisualRange = this->dataPtr->msg.range_max(); + this->dataPtr->minVisualRange = this->dataPtr->msg.range_min(); + this->dataPtr->lidar->SetMaxRange(this->dataPtr->maxVisualRange); + this->dataPtr->lidar->SetMinRange(this->dataPtr->minVisualRange); + this->MinRangeChanged(); + this->MaxRangeChanged(); + } + } + } + } +} + +////////////////////////////////////////////////// +void VisualizeLidar::ResetLidarVisual() +{ + this->dataPtr->lidar->ClearPoints(); +} + +////////////////////////////////////////////////// +QString VisualizeLidar::MaxRange() const +{ + return QString::fromStdString(std::to_string(this->dataPtr->maxVisualRange)); +} + +////////////////////////////////////////////////// +QString VisualizeLidar::MinRange() const +{ + return QString::fromStdString(std::to_string(this->dataPtr->minVisualRange)); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::VisualizeLidar, + ignition::gui::Plugin) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh new file mode 100644 index 0000000000..282def076e --- /dev/null +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2020 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_GAZEBO_GUI_VISUALIZELIDAR_HH_ +#define IGNITION_GAZEBO_GUI_VISUALIZELIDAR_HH_ + +#include + +#include "ignition/msgs/laserscan.pb.h" +#include "ignition/gazebo/gui/GuiSystem.hh" +#include "ignition/gui/qt.h" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + class VisualizeLidarPrivate; + + /// \brief Visualize the LaserScan message returned by the sensors. Use the + /// checkbox to turn visualization of non-hitting rays on or off and + /// the textfield to select the message to be visualised. The combobox is + /// used to select the type of visual for the sensor data. + class VisualizeLidar : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Message topic + Q_PROPERTY( + QString topicName + READ TopicName + WRITE SetTopicName + NOTIFY TopicNameChanged + ) + + /// \brief Min Range + Q_PROPERTY( + QString minRange + READ MinRange + NOTIFY MinRangeChanged + ) + + /// \brief Max Range + Q_PROPERTY( + QString maxRange + READ MaxRange + NOTIFY MaxRangeChanged + ) + + /// \brief Constructor + public: VisualizeLidar(); + + /// \brief Destructor + public: ~VisualizeLidar() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation Inherited + public: bool eventFilter(QObject *_obj, QEvent *_event); + + // Documentation inherited + public: void Update(const UpdateInfo &, + EntityComponentManager &_ecm) override; + + /// \brief Callback function to get data from the message + /// \param[in]_msg LidarSensor message + public: void OnScan(const msgs::LaserScan &_msg); + + /// \brief Reset and clear visual + void ResetLidarVisual(); + + /// \brief Load the scene and attach LidarVisual to the scene + public: void LoadLidar(); + + /// \brief Set visual type of LidarVisual + /// \param[in] _type Index of selected visual type + public slots: void UpdateType(int _type); + + /// \brief Set topic to subscribe for LidarSensor data + /// \param[in] _topicName Name of selected topic + public: Q_INVOKABLE void SetTopicName(const QString &_topicName); + + /// \brief Get the name of the topic currently beign visualised. + /// \return Name such as "/lidar" + public: Q_INVOKABLE QString TopicName() const; + + /// \brief Set whether to display non-hitting rays + /// \param[in] _value Boolean value for displaying non hitting rays + public slots: void UpdateNonHitting(bool _value); + + /// \brief Notify that topic name has changed + signals: void TopicNameChanged(); + + /// \brief Notify that topic name has changed + signals: void MinRangeChanged(); + + /// \brief Notify that topic name has changed + signals: void MaxRangeChanged(); + + /// \brief Get the maximum range of the lidar sensor (in metres). + /// \return Range, the maximum distance sensed by the sensor. + public: Q_INVOKABLE QString MaxRange() const; + + /// \brief Get the minimum range of the lidar sensor (in metres). + /// \return Range, the minimum distance sensed by the sensor. + public: Q_INVOKABLE QString MinRange() const; + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml new file mode 100644 index 0000000000..0d559ad667 --- /dev/null +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2020 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.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 6 + columnSpacing: 10 + Layout.minimumWidth: 250 + Layout.minimumHeight: 300 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + + // Left spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + CheckBox { + Layout.alignment: Qt.AlignHCenter + id: visualizeLidar + Layout.columnSpan: 4 + text: qsTr("Show Non Hitting Rays") + checked: true + onClicked: { + VisualizeLidar.UpdateNonHitting(checked) + } + } + + // Right spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + Text { + Layout.columnSpan: 2 + id: topicNameText + color: "dimgrey" + text: "Topic Name" + } + + TextField { + id: topicNameField + text: VisualizeLidar.topicName + onEditingFinished: VisualizeLidar.topicName = topicNameField.text + } + + // Right spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + Text { + Layout.columnSpan: 2 + id: minRangeText + color: "dimgrey" + text: "Minimum Range (m)" + } + + Text{ + id: minRangeField + text: VisualizeLidar.minRange + } + + Text { + Layout.columnSpan: 2 + id: maxRangeText + color: "dimgrey" + text: "Maximum Range (m)" + } + + Text{ + id: maxRangeField + text: VisualizeLidar.maxRange + } + + Text { + Layout.columnSpan: 2 + id: updatePeriodText + color: "dimgrey" + text: "Visual Type" + } + + ComboBox { + id: typeCombo + Layout.fillWidth: true + currentIndex: 3 + model: ["None", "Rays", "Points", "Triangles" ] + onCurrentIndexChanged: { + if (currentIndex < 0) { + return; + } + VisualizeLidar.UpdateType(typeCombo.currentIndex); + } + } +} diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.qrc b/src/gui/plugins/visualize_lidar/VisualizeLidar.qrc new file mode 100644 index 0000000000..a351b7c712 --- /dev/null +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.qrc @@ -0,0 +1,5 @@ + + + VisualizeLidar.qml + + \ No newline at end of file From 0b83885b17cbec6f0f703f74c0e7a1982272fa22 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 24 Aug 2020 14:50:56 +0530 Subject: [PATCH 2/7] addressed comments Signed-off-by: Mihir Kulkarni --- examples/worlds/visualize_lidar.sdf | 6 +- .../plugins/visualize_lidar/VisualizeLidar.cc | 156 +++++++++++++----- .../plugins/visualize_lidar/VisualizeLidar.hh | 40 +++-- .../visualize_lidar/VisualizeLidar.qml | 77 +++++---- 4 files changed, 187 insertions(+), 92 deletions(-) diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf index 325178eb59..3b12a901a1 100644 --- a/examples/worlds/visualize_lidar.sdf +++ b/examples/worlds/visualize_lidar.sdf @@ -233,7 +233,6 @@ true - true @@ -473,14 +472,11 @@ 0.3 1 - 0 0 0 0 0 1.57 - - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Playground - + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Playground diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 58df59f2bf..91f7b1e7d5 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -21,13 +21,8 @@ #include #include -#include #include -#include -#include #include -#include -#include #include @@ -82,7 +77,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE public: rendering::LidarVisualPtr lidar; /// \brief Visual type for lidar visual - public: rendering::LidarVisualType visualType; + public: rendering::LidarVisualType visualType{ + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS}; /// \brief URI sequence to the lidar link public: std::string lidarString{""}; @@ -97,16 +93,19 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE public: bool checkboxState{false}; /// \brief Topic name to subscribe - public: std::string topicName; + public: std::string topicName{""}; + + /// \brief List of topics publishing LaserScan messages. + public: QStringList topicList; /// \brief Entity representing the sensor in the world public: gazebo::Entity lidarEntity; /// \brief Minimum range for the visual - public: double minVisualRange; + public: double minVisualRange{0.0}; /// \brief Maximum range for the visual - public: double maxVisualRange; + public: double maxVisualRange{0.0}; /// \brief Mutex for variable mutated by the checkbox and spinboxes /// callbacks. @@ -121,7 +120,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE public: bool visualDirty{false}; /// \brief lidar sensor entity dirty flag - public: bool lidarEntityDirty{false}; + public: bool lidarEntityDirty{true}; /// \brief Name of the world public: std::string worldName; @@ -190,14 +189,16 @@ void VisualizeLidar::LoadLidar() this->dataPtr->lidar = scene->CreateLidarVisual(); if (!this->dataPtr->lidar) { - ignwarn << "Failed to create lidar, lidar visual plugin won't work." + ignwarn << "Failed to create lidar, visualize lidar plugin won't work." << std::endl; + this->dataPtr->lidar->Destroy(); + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->removeEventFilter(this); return; } - if (this->dataPtr->lidar) + else { root->AddChild(this->dataPtr->lidar); this->dataPtr->initialized = true; @@ -274,7 +275,7 @@ void VisualizeLidar::Update(const UpdateInfo &, { auto parent = baseEntity; bool success = false; - for (auto i = 0ul; i < lidarURIVec.size()-1; i++) + for (size_t i = 0u; i < lidarURIVec.size()-1; i++) { auto children = _ecm.EntitiesByComponents( components::ParentEntity(parent)); @@ -295,14 +296,14 @@ void VisualizeLidar::Update(const UpdateInfo &, break; } } - if (foundChild == false) + if (!foundChild) { ignerr << "The entity could not be found." << "Error displaying lidar visual" <dataPtr->lidarEntity = parent; this->dataPtr->lidarEntityDirty = false; @@ -310,7 +311,15 @@ void VisualizeLidar::Update(const UpdateInfo &, } } } - if (this->dataPtr->lidarEntity) + + // Only update lidarPose if the lidarEntity exists and the lidar is initialized + // and the sensor message is yet to arrive. + // + // If we update the worldpose on the physics thread **after** the sensor + // data arrives, the visual is offset from the obstacle if the sensor is + // moving fast. + if (!this->dataPtr->lidarEntityDirty && this->dataPtr->initialized && + !this->dataPtr->visualDirty) { this->dataPtr->lidarPose = worldPose(this->dataPtr->lidarEntity, _ecm); } @@ -320,31 +329,42 @@ void VisualizeLidar::Update(const UpdateInfo &, void VisualizeLidar::UpdateType(int _type) { std::lock_guard lock(this->dataPtr->serviceMutex); - switch (_type) { - case 0: this->dataPtr->visualType = - rendering::LidarVisualType::LVT_NONE; - break; - case 1: this->dataPtr->visualType = - rendering::LidarVisualType::LVT_RAY_LINES; - break; - case 2: this->dataPtr->visualType = - rendering::LidarVisualType::LVT_POINTS; - break; - case 3: this->dataPtr->visualType = - rendering::LidarVisualType::LVT_TRIANGLE_STRIPS; - break; - default: this->dataPtr->visualType = - rendering::LidarVisualType::LVT_TRIANGLE_STRIPS; - break; + switch (_type) + { + case 0: + this->dataPtr->visualType = rendering::LidarVisualType::LVT_NONE; + break; + + case 1: + this->dataPtr->visualType = rendering::LidarVisualType::LVT_RAY_LINES; + break; + + case 2: + this->dataPtr->visualType = rendering::LidarVisualType::LVT_POINTS; + break; + + case 3: + this->dataPtr->visualType = + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS; + break; + + default: + this->dataPtr->visualType = + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS; + break; } this->dataPtr->lidar->SetType(this->dataPtr->visualType); } ////////////////////////////////////////////////// -void VisualizeLidar::SetTopicName(const QString &_topicName) +void VisualizeLidar::OnTopic(const QString &_topicName) { std::lock_guard(this->dataPtr->serviceMutex); - this->dataPtr->node.Unsubscribe(this->dataPtr->topicName); + if (!this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) + { + ignerr << "Unable to unsubscribe from topic [" + << this->dataPtr->topicName <<"]" <dataPtr->topicName = _topicName.toStdString(); // Reset visualization @@ -354,25 +374,80 @@ void VisualizeLidar::SetTopicName(const QString &_topicName) if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName, &VisualizeLidar::OnScan, this)) { - ignerr << "Input subscriber could not be created for topic [" + ignerr << "Unable to subscribe to topic [" << this->dataPtr->topicName << "]\n"; return; } ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl; - this->TopicNameChanged(); } ////////////////////////////////////////////////// -QString VisualizeLidar::TopicName() const +void VisualizeLidar::UpdateNonHitting(bool _value) { - return QString::fromStdString(this->dataPtr->topicName); + std::lock_guard(this->dataPtr->serviceMutex); + this->dataPtr->lidar->SetDisplayNonHitting(_value); } ////////////////////////////////////////////////// -void VisualizeLidar::UpdateNonHitting(bool _value) +void VisualizeLidar::DisplayVisual(bool _value) { std::lock_guard(this->dataPtr->serviceMutex); - this->dataPtr->lidar->SetDisplayNonHitting(_value); + if (!_value) + { + ignmsg << "Lidar Visual Display OFF." << std::endl; + this->dataPtr->lidar->SetType(rendering::LidarVisualType::LVT_NONE); + } + else + { + ignmsg << "Lidar Visual Display ON." << std::endl; + this->dataPtr->lidar->SetType(this->dataPtr->visualType); + } +} + +///////////////////////////////////////////////// +void VisualizeLidar::OnRefresh() +{ + std::lock_guard(this->dataPtr->serviceMutex); + ignmsg << "Refreshing topic list for LaserScan messages." << std::endl; + + // Clear + this->dataPtr->topicList.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.LaserScan") + { + this->dataPtr->topicList.push_back(QString::fromStdString(topic)); + break; + } + } + } + if (this->dataPtr->topicList.size() > 0) + { + this->OnTopic(this->dataPtr->topicList.at(0)); + } + + this->TopicListChanged(); +} + +///////////////////////////////////////////////// +QStringList VisualizeLidar::TopicList() const +{ + return this->dataPtr->topicList; +} + +///////////////////////////////////////////////// +void VisualizeLidar::SetTopicList(const QStringList &_topicList) +{ + this->dataPtr->topicList = _topicList; + this->TopicListChanged(); } ////////////////////////////////////////////////// @@ -416,6 +491,7 @@ void VisualizeLidar::OnScan(const msgs::LaserScan &_msg) this->dataPtr->lidar->SetMinRange(this->dataPtr->minVisualRange); this->MinRangeChanged(); this->MaxRangeChanged(); + break; } } } diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh index 282def076e..a2d684643f 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -41,12 +41,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { Q_OBJECT - /// \brief Message topic + /// \brief Topic list Q_PROPERTY( - QString topicName - READ TopicName - WRITE SetTopicName - NOTIFY TopicNameChanged + QStringList topicList + READ TopicList + WRITE SetTopicList + NOTIFY TopicListChanged ) /// \brief Min Range @@ -93,25 +93,37 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \param[in] _type Index of selected visual type public slots: void UpdateType(int _type); + /// \brief Get the topic list as a string + /// \return Message type + public: Q_INVOKABLE QStringList TopicList() const; + + /// \brief Set the topic list from a string, for example + /// 'ignition.msgs.StringMsg' + /// \param[in] _topicList Message type + public: Q_INVOKABLE void SetTopicList(const QStringList &_topicList); + + /// \brief Notify that topic list has changed + signals: void TopicListChanged(); + /// \brief Set topic to subscribe for LidarSensor data /// \param[in] _topicName Name of selected topic - public: Q_INVOKABLE void SetTopicName(const QString &_topicName); - - /// \brief Get the name of the topic currently beign visualised. - /// \return Name such as "/lidar" - public: Q_INVOKABLE QString TopicName() const; + public: Q_INVOKABLE void OnTopic(const QString &_topicName); /// \brief Set whether to display non-hitting rays /// \param[in] _value Boolean value for displaying non hitting rays public slots: void UpdateNonHitting(bool _value); - /// \brief Notify that topic name has changed - signals: void TopicNameChanged(); + /// \brief Set whether to display the lidar visual + /// \param[in] _value Boolean value for displaying the visual + public slots: void DisplayVisual(bool _value); + + /// \brief Callback when refresh button is pressed. + public slots: void OnRefresh(); - /// \brief Notify that topic name has changed + /// \brief Notify that minimum range has changed signals: void MinRangeChanged(); - /// \brief Notify that topic name has changed + /// \brief Notify that maximum range has changed signals: void MaxRangeChanged(); /// \brief Get the maximum range of the lidar sensor (in metres). diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml index 0d559ad667..0d1281905f 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml @@ -17,30 +17,34 @@ import QtQuick 2.9 import QtQuick.Controls 2.1 import QtQuick.Dialogs 1.0 +import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 import "qrc:/qml" GridLayout { columns: 6 columnSpacing: 10 - Layout.minimumWidth: 250 + Layout.minimumWidth: 350 Layout.minimumHeight: 300 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 - - // Left spacer - Item { - Layout.columnSpan: 1 - Layout.rowSpan: 15 - Layout.fillWidth: true + CheckBox { + Layout.alignment: Qt.AlignHCenter + id: displayVisual + Layout.columnSpan: 3 + text: qsTr("Display Lidar Visual") + checked: true + onClicked: { + VisualizeLidar.DisplayVisual(checked) + } } CheckBox { Layout.alignment: Qt.AlignHCenter - id: visualizeLidar - Layout.columnSpan: 4 + id: displayNonHitting + Layout.columnSpan: 3 text: qsTr("Show Non Hitting Rays") checked: true onClicked: { @@ -48,41 +52,46 @@ GridLayout { } } - // Right spacer - Item { + RoundButton { Layout.columnSpan: 1 - Layout.rowSpan: 15 - Layout.fillWidth: true - } - - Text { - Layout.columnSpan: 2 - id: topicNameText - color: "dimgrey" - text: "Topic Name" - } - - TextField { - id: topicNameField - text: VisualizeLidar.topicName - onEditingFinished: VisualizeLidar.topicName = topicNameField.text + text: "\u21bb" + Material.background: Material.primary + onClicked: { + combo.currentIndex = 0 + VisualizeLidar.OnRefresh(); + } + ToolTip.visible: hovered + ToolTip.delay: tooltipDelay + ToolTip.timeout: tooltipTimeout + ToolTip.text: qsTr("Refresh list of topics publishing laser scans") } - // Right spacer - Item { - Layout.columnSpan: 1 - Layout.rowSpan: 15 + ComboBox { + Layout.columnSpan: 5 + id: combo Layout.fillWidth: true + model: VisualizeLidar.topicList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + VisualizeLidar.OnTopic(textAt(currentIndex)); + } + ToolTip.visible: hovered + ToolTip.delay: tooltipDelay + ToolTip.timeout: tooltipTimeout + ToolTip.text: qsTr("Ignition transport topics publishing LaserScan messages") } Text { Layout.columnSpan: 2 id: minRangeText color: "dimgrey" - text: "Minimum Range (m)" + text: "Min. Range (m)" } Text{ + Layout.columnSpan: 4 id: minRangeField text: VisualizeLidar.minRange } @@ -91,10 +100,11 @@ GridLayout { Layout.columnSpan: 2 id: maxRangeText color: "dimgrey" - text: "Maximum Range (m)" + text: "Max. Range (m)" } Text{ + Layout.columnSpan: 4 id: maxRangeField text: VisualizeLidar.maxRange } @@ -107,10 +117,11 @@ GridLayout { } ComboBox { + Layout.columnSpan: 4 id: typeCombo Layout.fillWidth: true currentIndex: 3 - model: ["None", "Rays", "Points", "Triangles" ] + model: ["None", "Rays", "Points", "Triangle Strips" ] onCurrentIndexChanged: { if (currentIndex < 0) { return; From cb9e9a7670f73d5441ce596115313bbf5e71a8db Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 24 Aug 2020 14:54:41 +0530 Subject: [PATCH 3/7] remove white line Signed-off-by: Mihir Kulkarni --- examples/worlds/visualize_lidar.sdf | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf index 3b12a901a1..aa1deca7f1 100644 --- a/examples/worlds/visualize_lidar.sdf +++ b/examples/worlds/visualize_lidar.sdf @@ -238,7 +238,6 @@ -4 0 0.325 0 0 0.0 - -0.151427 -0 0.175 0 -0 0 From 43f9b6bf300fe69f4524bc3adf9f66f9eff7a5f5 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 24 Aug 2020 17:23:21 +0530 Subject: [PATCH 4/7] moved ClearPoints() to rendering thread. other minor changes Signed-off-by: Mihir Kulkarni --- .../plugins/visualize_lidar/VisualizeLidar.cc | 17 ++++++++++------- .../plugins/visualize_lidar/VisualizeLidar.hh | 11 ++++------- .../plugins/visualize_lidar/VisualizeLidar.qml | 6 ++++-- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 91f7b1e7d5..181e1cd8dc 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -116,6 +116,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Initialization flag public: bool initialized{false}; + /// \brief Reset visual flag + public: bool resetVisual{false}; + /// \brief lidar visual display dirty flag public: bool visualDirty{false}; @@ -231,6 +234,11 @@ bool VisualizeLidar::eventFilter(QObject *_obj, QEvent *_event) if (this->dataPtr->lidar) { + if (this->dataPtr->resetVisual) + { + this->dataPtr->lidar->ClearPoints(); + this->dataPtr->resetVisual = false; + } if (this->dataPtr->visualDirty) { this->dataPtr->lidar->SetWorldPose(this->dataPtr->lidarPose); @@ -368,7 +376,7 @@ void VisualizeLidar::OnTopic(const QString &_topicName) this->dataPtr->topicName = _topicName.toStdString(); // Reset visualization - this->ResetLidarVisual(); + this->dataPtr->resetVisual = true; // Create new subscription if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName, @@ -378,6 +386,7 @@ void VisualizeLidar::OnTopic(const QString &_topicName) << this->dataPtr->topicName << "]\n"; return; } + this->dataPtr->visualDirty = false; ignmsg << "Subscribed to " << this->dataPtr->topicName << std::endl; } @@ -498,12 +507,6 @@ void VisualizeLidar::OnScan(const msgs::LaserScan &_msg) } } -////////////////////////////////////////////////// -void VisualizeLidar::ResetLidarVisual() -{ - this->dataPtr->lidar->ClearPoints(); -} - ////////////////////////////////////////////////// QString VisualizeLidar::MaxRange() const { diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh index a2d684643f..78144a3eb9 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -83,15 +83,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \param[in]_msg LidarSensor message public: void OnScan(const msgs::LaserScan &_msg); - /// \brief Reset and clear visual - void ResetLidarVisual(); - /// \brief Load the scene and attach LidarVisual to the scene public: void LoadLidar(); /// \brief Set visual type of LidarVisual /// \param[in] _type Index of selected visual type - public slots: void UpdateType(int _type); + public: Q_INVOKABLE void UpdateType(int _type); /// \brief Get the topic list as a string /// \return Message type @@ -111,14 +108,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Set whether to display non-hitting rays /// \param[in] _value Boolean value for displaying non hitting rays - public slots: void UpdateNonHitting(bool _value); + public: Q_INVOKABLE void UpdateNonHitting(bool _value); /// \brief Set whether to display the lidar visual /// \param[in] _value Boolean value for displaying the visual - public slots: void DisplayVisual(bool _value); + public: Q_INVOKABLE void DisplayVisual(bool _value); /// \brief Callback when refresh button is pressed. - public slots: void OnRefresh(); + public: Q_INVOKABLE void OnRefresh(); /// \brief Notify that minimum range has changed signals: void MinRangeChanged(); diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml index 0d1281905f..dbe9d680ee 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml @@ -33,7 +33,8 @@ GridLayout { CheckBox { Layout.alignment: Qt.AlignHCenter id: displayVisual - Layout.columnSpan: 3 + Layout.columnSpan: 6 + Layout.fillWidth: true text: qsTr("Display Lidar Visual") checked: true onClicked: { @@ -44,7 +45,8 @@ GridLayout { CheckBox { Layout.alignment: Qt.AlignHCenter id: displayNonHitting - Layout.columnSpan: 3 + Layout.columnSpan: 6 + Layout.fillWidth: true text: qsTr("Show Non Hitting Rays") checked: true onClicked: { From bd3499603d5ad25fb815e0075204bcc66bc3e8b0 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 25 Aug 2020 16:52:45 +0530 Subject: [PATCH 5/7] deleting visual during plugin destruction, and SetVisible() for displaying visual Signed-off-by: Mihir Kulkarni --- .../plugins/visualize_lidar/VisualizeLidar.cc | 36 ++++++++----------- 1 file changed, 14 insertions(+), 22 deletions(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 181e1cd8dc..a2077a7cc1 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -78,7 +78,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Visual type for lidar visual public: rendering::LidarVisualType visualType{ - rendering::LidarVisualType::LVT_TRIANGLE_STRIPS}; + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS}; /// \brief URI sequence to the lidar link public: std::string lidarString{""}; @@ -89,9 +89,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Pose of the lidar visual public: math::Pose3d lidarPose{math::Pose3d::Zero}; - /// \brief Current state of the checkbox - public: bool checkboxState{false}; - /// \brief Topic name to subscribe public: std::string topicName{""}; @@ -124,9 +121,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief lidar sensor entity dirty flag public: bool lidarEntityDirty{true}; - - /// \brief Name of the world - public: std::string worldName; }; } } @@ -143,7 +137,11 @@ VisualizeLidar::VisualizeLidar() } ///////////////////////////////////////////////// -VisualizeLidar::~VisualizeLidar() = default; +VisualizeLidar::~VisualizeLidar() +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->scene->DestroyVisual(this->dataPtr->lidar); +} ///////////////////////////////////////////////// void VisualizeLidar::LoadLidar() @@ -195,7 +193,7 @@ void VisualizeLidar::LoadLidar() ignwarn << "Failed to create lidar, visualize lidar plugin won't work." << std::endl; - this->dataPtr->lidar->Destroy(); + scene->DestroyVisual(this->dataPtr->lidar); ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->removeEventFilter(this); @@ -203,6 +201,7 @@ void VisualizeLidar::LoadLidar() } else { + this->dataPtr->scene = scene; root->AddChild(this->dataPtr->lidar); this->dataPtr->initialized = true; } @@ -320,8 +319,8 @@ void VisualizeLidar::Update(const UpdateInfo &, } } - // Only update lidarPose if the lidarEntity exists and the lidar is initialized - // and the sensor message is yet to arrive. + // Only update lidarPose if the lidarEntity exists and the lidar is + // initialized and the sensor message is yet to arrive. // // If we update the worldpose on the physics thread **after** the sensor // data arrives, the visual is offset from the obstacle if the sensor is @@ -371,7 +370,7 @@ void VisualizeLidar::OnTopic(const QString &_topicName) if (!this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) { ignerr << "Unable to unsubscribe from topic [" - << this->dataPtr->topicName <<"]" <dataPtr->topicName <<"]" <dataPtr->topicName = _topicName.toStdString(); @@ -401,16 +400,9 @@ void VisualizeLidar::UpdateNonHitting(bool _value) void VisualizeLidar::DisplayVisual(bool _value) { std::lock_guard(this->dataPtr->serviceMutex); - if (!_value) - { - ignmsg << "Lidar Visual Display OFF." << std::endl; - this->dataPtr->lidar->SetType(rendering::LidarVisualType::LVT_NONE); - } - else - { - ignmsg << "Lidar Visual Display ON." << std::endl; - this->dataPtr->lidar->SetType(this->dataPtr->visualType); - } + this->dataPtr->lidar->SetVisible(_value); + ignmsg << "Lidar Visual Display " << ((_value) ? "ON." : "OFF.") + << std::endl; } ///////////////////////////////////////////////// From 42d221dd35c5f07a99038f9ec64e39268d837105 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Fri, 4 Sep 2020 22:03:37 +0530 Subject: [PATCH 6/7] minor changes Signed-off-by: Mihir Kulkarni --- examples/worlds/visualize_lidar.sdf | 7 ------- src/gui/plugins/visualize_lidar/VisualizeLidar.cc | 1 - 2 files changed, 8 deletions(-) diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf index aa1deca7f1..311aa688c6 100644 --- a/examples/worlds/visualize_lidar.sdf +++ b/examples/worlds/visualize_lidar.sdf @@ -114,9 +114,6 @@ - 20 20 0.1 @@ -124,10 +121,6 @@ - 20 20 0.1 diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index a2077a7cc1..f98952be38 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -197,7 +197,6 @@ void VisualizeLidar::LoadLidar() ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->removeEventFilter(this); - return; } else { From 310a4176e3757a1a6df9eadb78248b6100a4e350 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 22 Sep 2020 22:57:42 +0530 Subject: [PATCH 7/7] remove override from eventFilter Co-authored-by: Louise Poubel --- src/gui/plugins/visualize_lidar/VisualizeLidar.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh index 78144a3eb9..e06dee0bd2 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -73,7 +73,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; // Documentation Inherited - public: bool eventFilter(QObject *_obj, QEvent *_event); + public: bool eventFilter(QObject *_obj, QEvent *_event) override; // Documentation inherited public: void Update(const UpdateInfo &,