diff --git a/ign_rviz/include/ignition/rviz/rviz.hpp b/ign_rviz/include/ignition/rviz/rviz.hpp index ba0059a..8e3cc04 100644 --- a/ign_rviz/include/ignition/rviz/rviz.hpp +++ b/ign_rviz/include/ignition/rviz/rviz.hpp @@ -66,20 +66,17 @@ class RViz : public QObject */ Q_INVOKABLE void addTFDisplay() { - try { - // Create new instance of plugin - DisplayPlugin tf_plugin = - std::dynamic_pointer_cast>( - plugin_loader.createSharedInstance( - "ignition/rviz/plugins/TFDisplay")); - tf_plugin->initialize(this->node); - tf_plugin->setFrameManager(this->frameManager); - tf_plugin->installEventFilter(ignition::gui::App()->findChild()); + // Load plugin + if (ignition::gui::App()->LoadPlugin("TFDisplay")) { + auto tfDisplayPlugins = + ignition::gui::App()->findChildren *>(); + int tfDisplayCount = tfDisplayPlugins.size() - 1; - // Add the new plugin to the list - tf_plugins.push_back(tf_plugin); - } catch (pluginlib::PluginlibException & ex) { - std::cout << ex.what() << std::endl; + // Set frame manager and install event filter for recently added plugin + tfDisplayPlugins[tfDisplayCount]->initialize(this->node); + tfDisplayPlugins[tfDisplayCount]->setFrameManager(this->frameManager); + ignition::gui::App()->findChild()->installEventFilter( + tfDisplayPlugins[tfDisplayCount]); } } diff --git a/ign_rviz_common/include/ignition/rviz/common/frame_manager.hpp b/ign_rviz_common/include/ignition/rviz/common/frame_manager.hpp index 3f56db0..1ad5d5e 100644 --- a/ign_rviz_common/include/ignition/rviz/common/frame_manager.hpp +++ b/ign_rviz_common/include/ignition/rviz/common/frame_manager.hpp @@ -60,7 +60,7 @@ class FrameManager : public QObject * @param[out] _pose: Frame pose * @return Pose validity (true if pose is valid, else false) */ - bool getFramePose(std::string & _frame, ignition::math::Pose3d & _pose); + bool getFramePose(const std::string & _frame, ignition::math::Pose3d & _pose); /** * @brief Get parent frame pose (position and orientation) @@ -68,7 +68,7 @@ class FrameManager : public QObject * @param[out] _pose: Parent frame pose * @return Pose validity (true if pose is valid, else false) */ - bool getParentPose(std::string & _child, ignition::math::Pose3d & _pose); + bool getParentPose(const std::string & _child, ignition::math::Pose3d & _pose); /** * @brief Get available tf frames diff --git a/ign_rviz_common/src/rviz/common/frame_manager.cpp b/ign_rviz_common/src/rviz/common/frame_manager.cpp index 43a7626..ffc12fd 100644 --- a/ign_rviz_common/src/rviz/common/frame_manager.cpp +++ b/ign_rviz_common/src/rviz/common/frame_manager.cpp @@ -138,7 +138,7 @@ void FrameManager::tf_callback(tf2_msgs::msg::TFMessage::SharedPtr _msg) } //////////////////////////////////////////////////////////////////////////////// -bool FrameManager::getFramePose(std::string & _frame, ignition::math::Pose3d & _pose) +bool FrameManager::getFramePose(const std::string & _frame, ignition::math::Pose3d & _pose) { std::lock_guard(this->tf_mutex_); @@ -153,7 +153,7 @@ bool FrameManager::getFramePose(std::string & _frame, ignition::math::Pose3d & _ } //////////////////////////////////////////////////////////////////////////////// -bool FrameManager::getParentPose(std::string & _child, ignition::math::Pose3d & _pose) +bool FrameManager::getParentPose(const std::string & _child, ignition::math::Pose3d & _pose) { std::lock_guard(this->tf_mutex_); diff --git a/ign_rviz_plugins/CMakeLists.txt b/ign_rviz_plugins/CMakeLists.txt index 6fc6d68..0758a7e 100644 --- a/ign_rviz_plugins/CMakeLists.txt +++ b/ign_rviz_plugins/CMakeLists.txt @@ -40,7 +40,6 @@ include_directories(SYSTEM qt5_add_resources(resources_RCC res/Plugins.qrc) ######################################################################## - add_library(AxesDisplay SHARED ${headers_MOC} include/ignition/rviz/plugins/AxesDisplay.hpp include/ignition/rviz/plugins/message_display_base.hpp @@ -67,7 +66,6 @@ target_include_directories(AxesDisplay ) ######################################################################## - add_library(GlobalOptions SHARED ${headers_MOC} include/ignition/rviz/plugins/GlobalOptions.hpp src/rviz/plugins/GlobalOptions.cpp @@ -93,16 +91,40 @@ target_include_directories(GlobalOptions ) ######################################################################## +add_library(TFDisplay SHARED ${headers_MOC} + include/ignition/rviz/plugins/TFDisplay.hpp + include/ignition/rviz/plugins/message_display_base.hpp + src/rviz/plugins/TFDisplay.cpp + ${resources_RCC} +) + +ament_target_dependencies(TFDisplay + tf2_ros + ignition-gui4 + ignition-math6 + ignition-rendering4 + ign_rviz_common +) + +target_link_libraries(TFDisplay Qt5::Core Qt5::Qml Qt5::Quick Qt5::QuickControls2) + +target_include_directories(TFDisplay + PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} + ${IGNITION-GUI_INCLUDE_DIRS} +) set(ign_rviz_plugins_headers_to_moc "include/ignition/rviz/plugins/message_display_base.hpp" - "include/ignition/rviz/plugins/tf_display.hpp" "include/ignition/rviz/plugins/laser_scan_display.hpp" ) +######################################################################## + add_library(ign_rviz_plugins SHARED ${ign_rviz_plugins_headers_to_moc} - src/rviz/plugins/tf_display.cpp src/rviz/plugins/laser_scan_display.cpp ) @@ -165,6 +187,16 @@ install( INCLUDES DESTINATION include ) +ament_export_libraries(TFDisplay) + +install( + TARGETS TFDisplay + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/ign_rviz_plugins/include/ignition/rviz/plugins/TFDisplay.hpp b/ign_rviz_plugins/include/ignition/rviz/plugins/TFDisplay.hpp new file mode 100644 index 0000000..59ed699 --- /dev/null +++ b/ign_rviz_plugins/include/ignition/rviz/plugins/TFDisplay.hpp @@ -0,0 +1,180 @@ +// Copyright (c) 2020 Open Source Robotics Foundation, Inc. +// +// 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__RVIZ__PLUGINS__TFDISPLAY_HPP_ +#define IGNITION__RVIZ__PLUGINS__TFDISPLAY_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include "ignition/rviz/plugins/message_display_base.hpp" + +namespace ignition +{ +namespace rviz +{ +namespace plugins +{ +//////////////////////////////////////////////////////////////////////////////// +class FrameModel : public QStandardItemModel +{ + Q_OBJECT + +public: + // Roles for tree view frames + enum FrameRoles + { + NameRole = Qt::UserRole + 1 + }; + + // Constructor + explicit FrameModel(QObject * _parent = 0); + + Q_INVOKABLE void addFrame(const QString & _name, QStandardItem * _parentItem); + Q_INVOKABLE QStandardItem * addParentRow(const QString & _name); + + QVariant data(const QModelIndex & _index, int _role = Qt::DisplayRole) const; + +protected: + QHash roleNames() const; +}; + +//////////////////////////////////////////////////////////////////////////////// +class TFDisplay : public MessageDisplay +{ + Q_OBJECT + +public: + /** + * Constructor for tf visualization plugin + */ + TFDisplay(); + + // Destructor + ~TFDisplay(); + + // Documentation inherited + void LoadConfig(const tinyxml2::XMLElement * /*_pluginElem*/); + + // Documentation Inherited + void initialize(rclcpp::Node::SharedPtr); + + // Documentation Inherited + void callback(const tf2_msgs::msg::TFMessage::SharedPtr) {} + + // Documentation inherited + void setTopic(std::string) {} + + /** + * @brief Qt eventFilters. Original documentation can be found + * here + */ + bool eventFilter(QObject *, QEvent *); + + // Documentation inherited + void setFrameManager(std::shared_ptr _frameManager); + + /** + * @brief Set axis visibility + * @param[in] _visible Axes visibility + */ + Q_INVOKABLE void showAxes(const bool & _visible); + + /** + * @brief Set arrow visibility + * @param[in] _visible Arrow visibility + */ + Q_INVOKABLE void showArrows(const bool & _visible); + + /** + * @brief Set frame name visibility + * @param[in] _visible Frame name visibility + */ + Q_INVOKABLE void showNames(const bool & _visible); + + /** + * @brief Set axes arrow head visibility + * @param[in] _visible Axes arrow head visibility + */ + Q_INVOKABLE void showAxesHead(const bool & _visible); + + /** + * @brief Set marker scale + * @param _scale TF visual marker scale + */ + Q_INVOKABLE void setMarkerScale(const float & _scale); + + /** + * @brief Set frame visibility + * @param[in] _frame Frame name + * @param[in] _visible Frame visibility + */ + Q_INVOKABLE void setFrameVisibility(const QString & _frame, const bool & _visible); + +protected: + /** + * @brief Create custom arrow visual for visualizing tf links + * @return tf arrow visual + */ + rendering::ArrowVisualPtr createTfArrow(); + + /** + * @brief Update tf visualization + */ + void updateTF(); + + /** + * @brief Creates a frame visual which includes an axis + * an arrow, and text visual + * @return A frame visual + */ + rendering::VisualPtr createVisualFrame(); + + /** + * @brief Update tree view and local frame list + */ + void refresh(); + +public: + // Tree view frame model + FrameModel * model; + +private: + ignition::rendering::AxisVisualPtr axis; + ignition::rendering::RenderEngine * engine; + ignition::rendering::ScenePtr scene; + ignition::rendering::VisualPtr tfRootVisual; + std::mutex lock; + bool axesVisible; + bool arrowsVisible; + bool namesVisible; + bool axesHeadVisible; + float markerScale; + QStandardItem * parentRow; + std::map frameInfo; +}; + +} // namespace plugins +} // namespace rviz +} // namespace ignition + +#endif // IGNITION__RVIZ__PLUGINS__TFDISPLAY_HPP_ diff --git a/ign_rviz_plugins/include/ignition/rviz/plugins/message_display_base.hpp b/ign_rviz_plugins/include/ignition/rviz/plugins/message_display_base.hpp index 7d81d24..dfe77bc 100644 --- a/ign_rviz_plugins/include/ignition/rviz/plugins/message_display_base.hpp +++ b/ign_rviz_plugins/include/ignition/rviz/plugins/message_display_base.hpp @@ -99,7 +99,9 @@ class MessageDisplay : public MessageDisplayBase rclcpp::Node::SharedPtr node; std::string topic_name; }; + } // namespace plugins } // namespace rviz } // namespace ignition + #endif // IGNITION__RVIZ__PLUGINS__MESSAGE_DISPLAY_BASE_HPP_ diff --git a/ign_rviz_plugins/include/ignition/rviz/plugins/tf_display.hpp b/ign_rviz_plugins/include/ignition/rviz/plugins/tf_display.hpp deleted file mode 100644 index 1f4d948..0000000 --- a/ign_rviz_plugins/include/ignition/rviz/plugins/tf_display.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright (c) 2020 Open Source Robotics Foundation, Inc. -// -// 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__RVIZ__PLUGINS__TF_DISPLAY_HPP_ -#define IGNITION__RVIZ__PLUGINS__TF_DISPLAY_HPP_ - -#include -#include - -#include -#include -#include -#include - -#include "ignition/rviz/plugins/message_display_base.hpp" - -namespace ignition -{ -namespace rviz -{ -namespace plugins -{ -class TFDisplay : public MessageDisplay -{ - Q_OBJECT - -public: - /** - * Constructor for tf visualization plugin - */ - TFDisplay(); - - ~TFDisplay(); - - // Documentation Inherited - void initialize(rclcpp::Node::SharedPtr); - - // Documentation Inherited - void callback(const tf2_msgs::msg::TFMessage::SharedPtr) {} - - // Documentation inherited - void setTopic(std::string); - - /** - * @brief Qt eventFilters. Original documentation can be found - * here - */ - bool eventFilter(QObject *, QEvent *); - - // Documentation inherited - void installEventFilter(ignition::gui::MainWindow *); - - // Documentation inherited - void setFrameManager(std::shared_ptr); - -protected: - /** - * @brief Create custom arrow visual for visualizing tf links - * @return tf arrow visual - */ - rendering::ArrowVisualPtr createTfArrow(); - - /** - * @brief Update tf visualization - */ - void updateTF(); - - /** - * @brief Creates a frame visual which includes an axis - * an arrow, and text visual - * @return A frame visual - */ - rendering::VisualPtr createVisualFrame(); - -private: - ignition::rendering::AxisVisualPtr axis; - ignition::rendering::RenderEngine * engine; - ignition::rendering::ScenePtr scene; - std::mutex lock; - ignition::rendering::VisualPtr tfRootVisual; -}; -} // namespace plugins -} // namespace rviz -} // namespace ignition -#endif // IGNITION__RVIZ__PLUGINS__TF_DISPLAY_HPP_ diff --git a/ign_rviz_plugins/res/Plugins.qrc b/ign_rviz_plugins/res/Plugins.qrc index 3982b6e..1da71bc 100644 --- a/ign_rviz_plugins/res/Plugins.qrc +++ b/ign_rviz_plugins/res/Plugins.qrc @@ -6,4 +6,8 @@ qml/AxesDisplay.qml + + + qml/TFDisplay.qml + diff --git a/ign_rviz_plugins/res/qml/TFDisplay.qml b/ign_rviz_plugins/res/qml/TFDisplay.qml new file mode 100644 index 0000000..a9f7af6 --- /dev/null +++ b/ign_rviz_plugins/res/qml/TFDisplay.qml @@ -0,0 +1,163 @@ +/* + * 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 QtQml.Models 2.2 +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Styles 1.4 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 + +Item { + // Tree Properties + property int rowHeight: 30; + + property color highlightColor: Material.accentColor; + + property color evenColor: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade100): + Material.color(Material.Grey, Material.Shade800); + + property color oddColor: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade200): + Material.color(Material.Grey, Material.Shade900); + + Layout.minimumWidth: 250 + Layout.minimumHeight: 500 + anchors.fill: parent + anchors.margins: 10 + + id: tfDisplay + + ColumnLayout { + width: parent.width + Layout.fillWidth: true + spacing: 2 + id: configColumn + + CheckBox { + checked: true + text: "Show Names" + onClicked: { TFDisplay.showNames(checked) } + } + + CheckBox { + checked: true + text: "Show Axes" + onClicked: { TFDisplay.showAxes(checked) } + } + + CheckBox { + checked: false + text: "Show Axes Head" + onClicked: { TFDisplay.showAxesHead(checked) } + } + + CheckBox { + checked: true + text: "Show Arrows" + onClicked: { TFDisplay.showArrows(checked) } + } + + RowLayout { + width: parent.width + spacing: 10 + + Text { + width: 80 + Layout.minimumWidth: 50 + text: "Marker Scale" + font.pointSize: 10.5 + } + + TextField { + id: markerScale + Layout.fillWidth: true + Layout.minimumWidth: 50 + width: 150 + placeholderText: "1.0" + + validator: RegExpValidator { + // Integer and floating point numbers + regExp: /^([0-9]*\.[0-9]+|[0-9]+)$/g + } + + onEditingFinished: { + TFDisplay.setMarkerScale(markerScale.text) + } + } + } + } + + TreeView { + id: tree + model: FrameModel + anchors.left: parent.left + anchors.right: parent.right + anchors.bottom: tfDisplay.bottom + anchors.top: configColumn.bottom + anchors.leftMargin: -10 + anchors.rightMargin: -10 + anchors.bottomMargin: -10 + anchors.topMargin: 10 + + Layout.fillWidth: true + Layout.fillHeight: true + width: parent.width + + headerVisible: false + headerDelegate: Rectangle { + visible: false + } + + frameVisible: false + + verticalScrollBarPolicy: Qt.ScrollBarAsNeeded + horizontalScrollBarPolicy: Qt.ScrollBarAsNeeded + + // Selection + selection: ItemSelectionModel { + model: tree.model; + } + + selectionMode: SelectionMode.SingleSelection; + + style: TreeViewStyle { + transientScrollBars: true + } + + TableViewColumn { + role: "name" + delegate: CheckDelegate { + text: model.name + checked: model.checked + onClicked: { + model.checked = checked; + TFDisplay.setFrameVisibility(model.name, model.checked); + } + } + } + + // Delegates + rowDelegate: Rectangle + { + id: row + color: (styleData.selected)? highlightColor : (styleData.row % 2 == 0) ? evenColor : oddColor; + height: rowHeight; + } + } +} \ No newline at end of file diff --git a/ign_rviz_plugins/src/rviz/plugins/TFDisplay.cpp b/ign_rviz_plugins/src/rviz/plugins/TFDisplay.cpp new file mode 100644 index 0000000..5d7bc32 --- /dev/null +++ b/ign_rviz_plugins/src/rviz/plugins/TFDisplay.cpp @@ -0,0 +1,410 @@ +// Copyright (c) 2020 Open Source Robotics Foundation, Inc. +// +// 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/rviz/plugins/TFDisplay.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ignition/rviz/common/rviz_events.hpp" + +namespace ignition +{ +namespace rviz +{ +namespace plugins +{ +#define MIN_FRAME_DISTANCE 0.25 +//////////////////////////////////////////////////////////////////////////////// +FrameModel::FrameModel(QObject * _parent) +: QStandardItemModel(_parent) +{} + +//////////////////////////////////////////////////////////////////////////////// +void FrameModel::addFrame(const QString & _name, QStandardItem * _parentItem) +{ + QStandardItem * frameRow = new QStandardItem(); + frameRow->setData(_name, NameRole); + frameRow->setCheckState(Qt::CheckState::Checked); + _parentItem->appendRow(frameRow); +} + +//////////////////////////////////////////////////////////////////////////////// +QStandardItem * FrameModel::addParentRow(const QString & _name) +{ + QStandardItem * entry = new QStandardItem(); + entry->setData(_name, NameRole); + entry->setCheckState(Qt::CheckState::Checked); + appendRow(entry); + return entry; +} + +//////////////////////////////////////////////////////////////////////////////// +QVariant FrameModel::data(const QModelIndex & _index, int _role) const +{ + QStandardItem * myItem = itemFromIndex(_index); + + if (_role == NameRole) { + return myItem->data(NameRole); + } + + if (_role == Qt::CheckStateRole) { + return myItem->data(Qt::CheckStateRole); + } + + return QVariant(); +} + +//////////////////////////////////////////////////////////////////////////////// +QHash FrameModel::roleNames() const +{ + QHash roles; + roles[NameRole] = "name"; + roles[Qt::CheckStateRole] = "checked"; + + return roles; +} + +//////////////////////////////////////////////////////////////////////////////// +TFDisplay::TFDisplay() +: MessageDisplay(), axesVisible(true), arrowsVisible(true), namesVisible(true), + axesHeadVisible(false), markerScale(0.4) +{ + // Get reference to scene + this->engine = ignition::rendering::engine("ogre"); + this->scene = this->engine->SceneByName("scene"); + + // Register pink material if not registered + rendering::MaterialPtr pink; + if (!this->scene->MaterialRegistered("Default/TransPink")) { + pink = this->scene->CreateMaterial("Default/TransPink"); + pink->SetAmbient(1.0, 0.0, 1.0); + pink->SetDiffuse(1.0, 0.0, 1.0); + pink->SetEmissive(1.0, 1.0, 0.0); + pink->SetTransparency(0.5); + pink->SetCastShadows(false); + pink->SetReceiveShadows(false); + pink->SetLightingEnabled(false); + } else { + pink = this->scene->Material("Default/TransPink"); + } + + // Default Yellow Material + rendering::MaterialPtr yellow = this->scene->Material("Default/TransYellow"); + + // Create a root visual for tf visualization + this->tfRootVisual = this->scene->CreateVisual(); + this->scene->RootVisual()->AddChild(tfRootVisual); + + this->model = new FrameModel(); + parentRow = this->model->addParentRow(QString::fromStdString("All Frames")); + + ignition::gui::App()->Engine()->rootContext()->setContextProperty("FrameModel", this->model); +} + +//////////////////////////////////////////////////////////////////////////////// +TFDisplay::~TFDisplay() +{ + std::lock_guard(this->lock); + // Delete visual + ignition::gui::App()->findChild()->removeEventFilter(this); + this->scene->DestroyVisual(this->tfRootVisual); +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::initialize(rclcpp::Node::SharedPtr _node) +{ + this->node = std::move(_node); +} + +//////////////////////////////////////////////////////////////////////////////// +rendering::ArrowVisualPtr TFDisplay::createTfArrow() +{ + // Create an arrow + rendering::ArrowVisualPtr arrow = this->scene->CreateArrowVisual(); + + // Set arrow head material to pink + rendering::VisualPtr head = arrow->Head(); + head->SetMaterial(this->scene->Material("Default/TransPink")); + + // Set arrow shaft material to yellow + rendering::VisualPtr shaft = arrow->Shaft(); + shaft->SetMaterial(this->scene->Material("Default/TransYellow")); + + // Set arrow (head and shaft) to unit length + shaft->Scale(0.5, 0.5, 1.5); + arrow->SetOrigin(0, 0, -0.75); + + return arrow; +} + +//////////////////////////////////////////////////////////////////////////////// +rendering::VisualPtr TFDisplay::createVisualFrame() +{ + rendering::VisualPtr visualFrame = this->scene->CreateVisual(); + + // Add axis + rendering::AxisVisualPtr axis = this->scene->CreateAxisVisual(); + axis->SetLocalScale(this->markerScale); + visualFrame->AddChild(axis); + + // Add arrow + rendering::ArrowVisualPtr arrow = this->createTfArrow(); + arrow->SetLocalPosition(0, 0, 0); + arrow->SetVisible(false); + + visualFrame->AddChild(arrow); + + // Add text + rendering::TextPtr frameName = this->scene->CreateText(); + frameName->SetTextString("frame"); + frameName->SetShowOnTop(true); + frameName->SetTextAlignment( + rendering::TextHorizontalAlign::CENTER, + rendering::TextVerticalAlign::CENTER); + frameName->SetCharHeight(0.15); + visualFrame->AddGeometry(frameName); + + return visualFrame; +} + +//////////////////////////////////////////////////////////////////////////////// +/** + * Update tf visualization only when ign::gui render event is received + */ +bool TFDisplay::eventFilter(QObject * _object, QEvent * _event) +{ + if (_event->type() == gui::events::Render::kType) { + // Create a default visual frame + if ((static_cast(this->tfRootVisual->ChildCount()) == 0) && this->frameManager) { + rendering::VisualPtr visualFrame = this->createVisualFrame(); + + // Display Axis with fixed frame name + rendering::TextPtr frameName = std::dynamic_pointer_cast( + visualFrame->GeometryByIndex(0)); + frameName->SetTextString(this->frameManager->getFixedFrame()); + + tfRootVisual->AddChild(visualFrame); + } + + updateTF(); + } + + if (_event->type() == rviz::events::FrameListChanged::kType) { + // Refresh Tree View + this->refresh(); + } + + return QObject::eventFilter(_object, _event); +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::updateTF() +{ + std::lock_guard(this->lock); + + // Create tf visual frames + for (int i = tfRootVisual->ChildCount(); i < static_cast(frameInfo.size()); ++i) { + rendering::VisualPtr visualFrame = this->createVisualFrame(); + this->tfRootVisual->AddChild(visualFrame); + } + + int i = -1; + // Update tf visual frames + for (const auto & frame : frameInfo) { + i++; + ignition::math::Pose3d pose, parentPose; + + rendering::VisualPtr visualFrame = std::dynamic_pointer_cast( + this->tfRootVisual->ChildByIndex(i)); + + // Set frame visibility + visualFrame->SetVisible(frame.second); + + // Skip processing if frame not visible + if (!frame.second) { + continue; + } + + // Set frame text + rendering::TextPtr frameName = std::dynamic_pointer_cast( + visualFrame->GeometryByIndex(0)); + frameName->SetTextString(frame.first); + + visualFrame->SetVisible(this->namesVisible); + + // Set frame position + if (this->frameManager->getFramePose(frame.first, pose)) { + visualFrame->SetLocalPosition(pose.Pos()); + } + + // Set axis orientation + rendering::AxisVisualPtr axis = std::dynamic_pointer_cast( + visualFrame->ChildByIndex(1)); + axis->SetLocalRotation(pose.Rot()); + axis->SetLocalScale(this->markerScale); + axis->SetVisible(this->axesVisible); + for (int i = 0; i < 3 && this->axesVisible; ++i) { + auto arrow = std::dynamic_pointer_cast(axis->ChildByIndex(i)); + arrow->ShowArrowHead(this->axesHeadVisible); + } + + // Get parent pose for tf links + bool result = this->frameManager->getParentPose(frame.first, parentPose); + rendering::ArrowVisualPtr arrow = std::dynamic_pointer_cast( + visualFrame->ChildByIndex(0)); + if (result) { + // Get direction and distance from child to parent frame + math::Vector3d dir = parentPose.Pos() - pose.Pos(); + double dist = dir.Length(); + + if (dist >= MIN_FRAME_DISTANCE) { + // Update tf arrow visual orientation to point fron child to parent frame + arrow->SetVisible(this->arrowsVisible); + + math::Quaterniond quat; + quat.From2Axes(-math::Vector3d::UnitZ, dir); + quat *= math::Quaterniond::EulerToQuaternion(M_PI, 0, 0); + arrow->SetLocalRotation(quat); + arrow->SetLocalScale(this->markerScale, this->markerScale, dist); + } else { + arrow->SetVisible(false); + } + } else { + arrow->SetVisible(false); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::setFrameManager(std::shared_ptr _frameManager) +{ + this->frameManager = std::move(_frameManager); + + // Refresh Tree View + this->refresh(); +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::refresh() +{ + std::lock_guard(this->lock); + + std::vector frames; + this->frameManager->getFrames(frames); + + if (frames.size() > 0) { + for (const auto frame : frames) { + this->frameInfo.insert({frame, true}); + } + + // Clear rows + parentRow->removeRows(0, parentRow->rowCount()); + + for (auto frame : frameInfo) { + this->model->addFrame(QString::fromStdString(frame.first), parentRow); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::LoadConfig(const tinyxml2::XMLElement * /*_pluginElem*/) +{ + if (this->title.empty()) { + this->title = "TF"; + } +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::showAxes(const bool & _visible) +{ + std::lock_guard(this->lock); + this->axesVisible = _visible; +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::showNames(const bool & _visible) +{ + std::lock_guard(this->lock); + this->namesVisible = _visible; +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::showArrows(const bool & _visible) +{ + std::lock_guard(this->lock); + this->arrowsVisible = _visible; +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::showAxesHead(const bool & _visible) +{ + std::lock_guard(this->lock); + this->axesHeadVisible = _visible; +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::setMarkerScale(const float & _scale) +{ + std::lock_guard(this->lock); + this->markerScale = _scale * 0.4; +} + +//////////////////////////////////////////////////////////////////////////////// +void TFDisplay::setFrameVisibility(const QString & _frame, const bool & _visible) +{ + std::lock_guard(this->lock); + + if (_frame == "All Frames") { + // Update frame GUI checkboxes + for (int i = 0; i < parentRow->rowCount(); ++i) { + parentRow->child(i)->setData(_visible, Qt::CheckStateRole); + } + // Update frame visibility info + for (auto & frame : frameInfo) { + frame.second = _visible; + } + + return; + } + + // Update frame visibility + bool frameStatus = true; + for (auto & frame : frameInfo) { + if (frame.first == _frame.toStdString()) { + frame.second = _visible; + } + frameStatus &= frame.second; + } + + // All Frames checkbox checked if all child frames are visible + parentRow->setData(frameStatus, Qt::CheckStateRole); +} + +} // namespace plugins +} // namespace rviz +} // namespace ignition + +IGNITION_ADD_PLUGIN( + ignition::rviz::plugins::TFDisplay, + ignition::gui::Plugin) diff --git a/ign_rviz_plugins/src/rviz/plugins/tf_display.cpp b/ign_rviz_plugins/src/rviz/plugins/tf_display.cpp deleted file mode 100644 index 8afda73..0000000 --- a/ign_rviz_plugins/src/rviz/plugins/tf_display.cpp +++ /dev/null @@ -1,242 +0,0 @@ -// Copyright (c) 2020 Open Source Robotics Foundation, Inc. -// -// 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/rviz/plugins/tf_display.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace ignition -{ -namespace rviz -{ -namespace plugins -{ -#define MIN_FRAME_DISTANCE 0.25 - -//////////////////////////////////////////////////////////////////////////////// -TFDisplay::TFDisplay() -: MessageDisplay() -{ - // Get reference to scene - this->engine = ignition::rendering::engine("ogre"); - this->scene = this->engine->SceneByName("scene"); - - // Register pink material if not registered - rendering::MaterialPtr pink; - if (!this->scene->MaterialRegistered("Default/TransPink")) { - pink = this->scene->CreateMaterial("Default/TransPink"); - pink->SetAmbient(1.0, 0.0, 1.0); - pink->SetDiffuse(1.0, 0.0, 1.0); - pink->SetEmissive(1.0, 1.0, 0.0); - pink->SetTransparency(0.5); - pink->SetCastShadows(false); - pink->SetReceiveShadows(false); - pink->SetLightingEnabled(false); - } else { - pink = this->scene->Material("Default/TransPink"); - } - - // Default Yellow Material - rendering::MaterialPtr yellow = this->scene->Material("Default/TransYellow"); - - // Create a root visual for tf visualization - this->tfRootVisual = this->scene->CreateVisual(); - this->scene->RootVisual()->AddChild(tfRootVisual); -} - -//////////////////////////////////////////////////////////////////////////////// -TFDisplay::~TFDisplay() -{} - -//////////////////////////////////////////////////////////////////////////////// -void TFDisplay::initialize(rclcpp::Node::SharedPtr node) -{ - this->node = std::move(node); -} - -//////////////////////////////////////////////////////////////////////////////// -void TFDisplay::setTopic(std::string topic_name) -{ - this->topic_name = topic_name; - this->subscriber = this->node->create_subscription( - this->topic_name, 10, - std::bind(&TFDisplay::callback, this, std::placeholders::_1)); -} - -//////////////////////////////////////////////////////////////////////////////// -rendering::ArrowVisualPtr TFDisplay::createTfArrow() -{ - // Create an arrow - rendering::ArrowVisualPtr arrow = this->scene->CreateArrowVisual(); - - // Set arrow head material to pink - rendering::VisualPtr head = arrow->Head(); - head->SetMaterial(this->scene->Material("Default/TransPink")); - - // Set arrow shaft material to yellow - rendering::VisualPtr shaft = arrow->Shaft(); - shaft->SetMaterial(this->scene->Material("Default/TransYellow")); - - // Set arrow (head and shaft) to unit length - shaft->Scale(0.5, 0.5, 1.5); - arrow->SetOrigin(0, 0, -0.75); - - return arrow; -} - -//////////////////////////////////////////////////////////////////////////////// -rendering::VisualPtr TFDisplay::createVisualFrame() -{ - rendering::VisualPtr visualFrame = this->scene->CreateVisual(); - - // Add axis - rendering::AxisVisualPtr axis = this->scene->CreateAxisVisual(); - axis->SetLocalScale(0.5); - visualFrame->AddChild(axis); - - // Add arrow - rendering::ArrowVisualPtr arrow = this->createTfArrow(); - arrow->SetLocalPosition(0, 0, 0); - arrow->SetVisible(false); - - visualFrame->AddChild(arrow); - - // Add text - rendering::TextPtr frameName = this->scene->CreateText(); - frameName->SetTextString("frame"); - frameName->SetShowOnTop(true); - frameName->SetTextAlignment( - rendering::TextHorizontalAlign::CENTER, - rendering::TextVerticalAlign::CENTER); - frameName->SetCharHeight(0.15); - visualFrame->AddGeometry(frameName); - - return visualFrame; -} - -//////////////////////////////////////////////////////////////////////////////// -/** - * Update tf visualization only when ign::gui render event is received - */ -bool TFDisplay::eventFilter(QObject * object, QEvent * event) -{ - if (event->type() == gui::events::Render::kType) { - // Create a default visual frame - if ((static_cast(this->tfRootVisual->ChildCount()) == 0) && this->frameManager) { - rendering::VisualPtr visualFrame = this->createVisualFrame(); - - // Display Axis with fixed frame name - rendering::TextPtr frameName = std::dynamic_pointer_cast( - visualFrame->GeometryByIndex(0)); - frameName->SetTextString(this->frameManager->getFixedFrame()); - - tfRootVisual->AddChild(visualFrame); - } - - updateTF(); - } - - return QObject::eventFilter(object, event); -} - -//////////////////////////////////////////////////////////////////////////////// -void TFDisplay::updateTF() -{ - std::lock_guard(this->lock); - - // Get available tf frames - std::vector frameIds; - frameManager->getFrames(frameIds); - - // Create tf visual frames - for (int i = tfRootVisual->ChildCount(); i < static_cast(frameIds.size()); ++i) { - rendering::VisualPtr visualFrame = this->createVisualFrame(); - this->tfRootVisual->AddChild(visualFrame); - } - - // Update tf visual frames - for (int i = 0; i < static_cast(frameIds.size()); ++i) { - ignition::math::Pose3d pose, parentPose; - - rendering::VisualPtr visualFrame = std::dynamic_pointer_cast( - this->tfRootVisual->ChildByIndex(i)); - - bool result = this->frameManager->getFramePose(frameIds[i], pose); - - // Set frame text - rendering::TextPtr frameName = std::dynamic_pointer_cast( - visualFrame->GeometryByIndex(0)); - frameName->SetTextString(frameIds[i]); - - // Set frame position - visualFrame->SetLocalPosition(pose.Pos()); - - // Set axis orientation - rendering::AxisVisualPtr axis = std::dynamic_pointer_cast( - visualFrame->ChildByIndex(1)); - axis->SetLocalRotation(pose.Rot()); - - // Get parent pose for tf links - result = this->frameManager->getParentPose(frameIds[i], parentPose); - if (result) { - rendering::ArrowVisualPtr arrow = std::dynamic_pointer_cast( - visualFrame->ChildByIndex( - 0)); - - // Get direction and distance from child to parent frame - math::Vector3d dir = parentPose.Pos() - pose.Pos(); - double dist = dir.Length(); - - if (dist >= MIN_FRAME_DISTANCE) { - // Update tf arrow visual orientation to point fron child to parent frame - arrow->SetVisible(true); - math::Quaterniond quat; - quat.From2Axes(-math::Vector3d::UnitZ, dir); - quat *= math::Quaterniond::EulerToQuaternion(M_PI, 0, 0); - arrow->SetLocalRotation(quat); - arrow->SetLocalScale(1, 1, dist); - } else { - arrow->SetVisible(false); - } - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -void TFDisplay::installEventFilter(ignition::gui::MainWindow * window) -{ - window->installEventFilter(this); -} - -//////////////////////////////////////////////////////////////////////////////// -void TFDisplay::setFrameManager(std::shared_ptr frameManager) -{ - this->frameManager = std::move(frameManager); -} - -} // namespace plugins -} // namespace rviz -} // namespace ignition - -PLUGINLIB_EXPORT_CLASS( - ignition::rviz::plugins::TFDisplay, - ignition::rviz::plugins::MessageDisplayBase)