From e596e3216d4a4393c5b9d79ab74dc44e45dcf0f8 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Fri, 3 Feb 2023 01:32:47 -0500 Subject: [PATCH] Create Follow Config plugin for camera following. Allows for follow camera control set from sdf as well as gui. Signed-off-by: Benjamin Perseghetti Co-authored-by: Jenn Nguyen --- src/plugins/CMakeLists.txt | 1 + src/plugins/camera_tracking/CameraTracking.cc | 8 +- src/plugins/follow_config/CMakeLists.txt | 8 + src/plugins/follow_config/FollowConfig.cc | 253 ++++++++++++++++++ src/plugins/follow_config/FollowConfig.hh | 68 +++++ src/plugins/follow_config/FollowConfig.qml | 123 +++++++++ src/plugins/follow_config/FollowConfig.qrc | 5 + 7 files changed, 464 insertions(+), 2 deletions(-) create mode 100644 src/plugins/follow_config/CMakeLists.txt create mode 100644 src/plugins/follow_config/FollowConfig.cc create mode 100644 src/plugins/follow_config/FollowConfig.hh create mode 100644 src/plugins/follow_config/FollowConfig.qml create mode 100644 src/plugins/follow_config/FollowConfig.qrc diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index ff282d6b9..e05a6f39b 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -113,6 +113,7 @@ endfunction() # Plugins add_subdirectory(camera_tracking) +add_subdirectory(follow_config) add_subdirectory(grid_config) add_subdirectory(image_display) add_subdirectory(interactive_view_control) diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index 6df3fe677..a6a24fe2a 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2023 Rudis Laboratories LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -289,8 +290,11 @@ bool CameraTrackingPrivate::OnFollowPGain(const msgs::Double &_msg, msgs::Boolean &_res) { std::lock_guard lock(this->mutex); - this->followPGain = msgs::Convert(_msg); - + if (!this->followTarget.empty()) + { + this->newFollowOffset = true; + this->followPGain = msgs::Convert(_msg); + } _res.set_data(true); return true; } diff --git a/src/plugins/follow_config/CMakeLists.txt b/src/plugins/follow_config/CMakeLists.txt new file mode 100644 index 000000000..727a7a6c9 --- /dev/null +++ b/src/plugins/follow_config/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_gui_add_plugin(FollowConfig + SOURCES + FollowConfig.cc + QT_HEADERS + FollowConfig.hh + TEST_SOURCES + # FollowConfig_TEST.cc +) diff --git a/src/plugins/follow_config/FollowConfig.cc b/src/plugins/follow_config/FollowConfig.cc new file mode 100644 index 000000000..7c182e029 --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.cc @@ -0,0 +1,253 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * 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 "gz/gui/Application.hh" +#include "gz/gui/Conversions.hh" +#include "gz/gui/GuiEvents.hh" +#include "gz/gui/MainWindow.hh" + +#include + +#include "FollowConfig.hh" + +/// \brief Private data class for FollowConfig +class gz::gui::plugins::FollowConfigPrivate +{ + public: std::string followTargetNameService; + + public: std::string followOffsetService; + + public: std::string followPGainService; + + /// \brief Offset of camera from target being followed + public: math::Vector3d followOffset{math::Vector3d(-5.0, 0.0, 3.0)}; + + /// \brief Follow P gain + public: double followPGain{0.01}; + + public: std::string followTargetName; + + public: transport::Node node; + + public: void UpdateFollowTargetName(); + + public: void UpdateFollowOffset(); + + public: void UpdateFollowPGain(); + + public: bool newFollowUpdateTargetName = false; + + public: bool newFollowUpdatePGain = false; + + public: bool newFollowUpdateOffset = false; + +}; + +using namespace gz; +using namespace gui; +using namespace plugins; + +///////////////////////////////////////////////// +FollowConfig::FollowConfig() + : gz::gui::Plugin(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +FollowConfig::~FollowConfig() = default; + +///////////////////////////////////////////////// +void FollowConfig::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Follow Config"; + + // Follow target name service + this->dataPtr->followTargetNameService = "/gui/follow"; + gzmsg << "FollowConfig: Follow target name service on [" + << this->dataPtr->followTargetNameService << "]" << std::endl; + + // Follow target offset service + this->dataPtr->followOffsetService = "/gui/follow/offset"; + gzmsg << "FollowConfig: Follow offset service on [" + << this->dataPtr->followOffsetService << "]" << std::endl; + + // Follow target pgain service + this->dataPtr->followPGainService = "/gui/follow/pgain"; + gzmsg << "FollowConfig: Follow P gain service on [" + << this->dataPtr->followPGainService << "]" << std::endl; + + + // Read configuration + if (_pluginElem) + { + if (auto nameElem = _pluginElem->FirstChildElement("follow_target")) + { + this->dataPtr->followTargetName = nameElem->GetText(); + gzmsg << "FollowConfig: Loaded follow_target from sdf [" + << this->dataPtr->followTargetName << "]" << std::endl; + this->dataPtr->newFollowUpdateTargetName = true; + } + if (auto offsetElem = _pluginElem->FirstChildElement("follow_offset")) + { + std::stringstream offsetStr; + offsetStr << std::string(offsetElem->GetText()); + offsetStr >> this->dataPtr->followOffset; + gzmsg << "FollowConfig: Loaded follow_offset from sdf [" + << this->dataPtr->followOffset << "]" << std::endl; + this->dataPtr->newFollowUpdateOffset = true; + } + if (auto pGainElem = _pluginElem->FirstChildElement("follow_pgain")) + { + this->dataPtr->followPGain = std::stod(std::string(pGainElem->GetText())); + gzmsg << "FollowConfig: Loaded follow_pgain from sdf [" + << this->dataPtr->followPGain << "]" << std::endl; + this->dataPtr->newFollowUpdatePGain = true; + } + } + + gui::App()->findChild< + MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool FollowConfig::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == events::Render::kType) + { + if (this->dataPtr->newFollowUpdateTargetName) + { + this->dataPtr->UpdateFollowTargetName(); + } + if (this->dataPtr->newFollowUpdatePGain) + { + this->dataPtr->UpdateFollowPGain(); + } + if (this->dataPtr->newFollowUpdateOffset) + { + this->dataPtr->UpdateFollowOffset(); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void FollowConfig::SetFollowOffset(double _x, + double _y, double _z) +{ + if (! this->dataPtr->newFollowUpdateOffset) + { + this->dataPtr->followOffset = math::Vector3d( + _x, _y, _z); + gzmsg << "FollowConfig: SetFollowOffset(" + << this->dataPtr->followOffset << ")" << std::endl; + this->dataPtr->newFollowUpdateOffset = true; + } +} + +///////////////////////////////////////////////// +void FollowConfig::SetFollowPGain(double _p) +{ + if (! this->dataPtr->newFollowUpdatePGain) + { + this->dataPtr->followPGain = _p; + gzmsg << "FollowConfig: SetFollowPGain(" + << this->dataPtr->followPGain << ")" << std::endl; + this->dataPtr->newFollowUpdatePGain = true; + } +} + +///////////////////////////////////////////////// +void FollowConfigPrivate::UpdateFollowTargetName() +{ + // Offset + std::function cbName = + [&](const msgs::Boolean &/*_rep*/, const bool _resultName) + { + if (!_resultName) { + gzerr << "FollowConfig: Error sending follow target name." << std::endl; + } else { + gzmsg << "FollowConfig: Request Target Name: " + << this->followTargetName << " sent" << std::endl; + } + }; + + msgs::StringMsg reqName; + reqName.set_data(this->followTargetName); + node.Request(this->followTargetNameService, reqName, cbName); + this->newFollowUpdateTargetName = false; +} + +///////////////////////////////////////////////// +void FollowConfigPrivate::UpdateFollowOffset() +{ + // Offset + std::function cbOffset = + [&](const msgs::Boolean &/*_rep*/, const bool _resultOffset) + { + if (!_resultOffset) { + gzerr << "FollowConfig: Error sending follow offset." << std::endl; + } else { + gzmsg << "FollowConfig: Request Offset: " + << this->followOffset << " sent" << std::endl; + } + }; + + msgs::Vector3d reqOffset; + reqOffset.set_x(this->followOffset.X()); + reqOffset.set_y(this->followOffset.Y()); + reqOffset.set_z(this->followOffset.Z()); + node.Request(this->followOffsetService, reqOffset, cbOffset); + this->newFollowUpdateOffset = false; +} + +///////////////////////////////////////////////// +void FollowConfigPrivate::UpdateFollowPGain() +{ + // PGain + std::function cbPGain = + [&](const msgs::Boolean &/*_rep*/, const bool _resultPGain) + { + if (!_resultPGain) { + gzerr << "FollowConfig: Error sending follow pgain." << std::endl; + } else { + gzmsg << "FollowConfig: Request PGain: " + << this->followPGain << " sent" << std::endl; + } + }; + + msgs::Double reqPGain; + reqPGain.set_data(this->followPGain); + node.Request(this->followPGainService, reqPGain, cbPGain); + this->newFollowUpdatePGain = false; +} + +// Register this plugin +GZ_ADD_PLUGIN(FollowConfig, + gui::Plugin) diff --git a/src/plugins/follow_config/FollowConfig.hh b/src/plugins/follow_config/FollowConfig.hh new file mode 100644 index 000000000..e4f2a02c4 --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * 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 GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_ +#define GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_ + +#include + +#include "gz/gui/Plugin.hh" + +namespace gz +{ +namespace gui +{ +namespace plugins +{ + class FollowConfigPrivate; + + class FollowConfig : public Plugin + { + Q_OBJECT + + /// \brief Constructor + public: FollowConfig(); + + /// \brief Destructor + public: virtual ~FollowConfig(); + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + /// \brief Set the follow offset, requested from the GUI. + /// \param[in] _x The follow offset distance in x + /// \param[in] _y The follow offset distance in y + /// \param[in] _z The follow offset distance in z + public slots: void SetFollowOffset(double _x, + double _y, double _z); + + /// \brief Set the follow pgain, requested from the GUI. + /// \param[in] _p The follow offset distance in x + public slots: void SetFollowPGain(double _p); + + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/plugins/follow_config/FollowConfig.qml b/src/plugins/follow_config/FollowConfig.qml new file mode 100644 index 000000000..f9eff9b9c --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.qml @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * 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.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Controls.Styles 1.4 +import QtQuick.Layouts 1.3 +import gz.gui 1.0 + +ColumnLayout { + Layout.minimumWidth: 200 + Layout.minimumHeight: 200 + Layout.margins: 2 + anchors.fill: parent + focus: true + + // X camera follow distance + property double xFollowOffset: -5.0 + // Y camera follow distance + property double yFollowOffset: 0.0 + // Z camera follow distance + property double zFollowOffset: 3.0 + // P Gain camera follow distance + property double pGainFollow: 0.01 + + GridLayout { + Layout.fillWidth: true + Layout.margins: 2 + columns: 2 + + // X follow offset distance + Label { + id: xFollowOffsetLabel + text: "Camera follow X (m)" + color: "dimgrey" + } + GzSpinBox { + id: xFollowOffsetField + Layout.fillWidth: true + value: xFollowOffset + maximumValue: 10.0 + minimumValue: -10.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + xFollowOffset = value + FollowConfig.SetFollowOffset(xFollowOffset, yFollowOffset, zFollowOffset) + } + } + // Y follow offset distance + Label { + id: yFollowOffsetLabel + text: "Camera follow Y (m)" + color: "dimgrey" + } + GzSpinBox { + id: yFollowOffsetField + Layout.fillWidth: true + value: yFollowOffset + maximumValue: 10.0 + minimumValue: -10.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + yFollowOffset = value + FollowConfig.SetFollowOffset(xFollowOffset, yFollowOffset, zFollowOffset) + } + } + // Z follow offset distance + Label { + id: zFollowOffsetLabel + text: "Camera follow Z (m)" + color: "dimgrey" + } + GzSpinBox { + id: zFollowOffsetField + Layout.fillWidth: true + value: zFollowOffset + maximumValue: 10.0 + minimumValue: -10.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + zFollowOffset = value + FollowConfig.SetFollowOffset(xFollowOffset, yFollowOffset, zFollowOffset) + } + } + // P Gain follow + Label { + id: pGainFollowLabel + text: "Camera follow P Gain" + color: "dimgrey" + } + GzSpinBox { + id: pGainFollowField + Layout.fillWidth: true + value: pGainFollow + maximumValue: 1.0 + minimumValue: 0.001 + decimals: 3 + stepSize: 0.01 + onEditingFinished:{ + pGainFollow = value + FollowConfig.SetFollowPGain(pGainFollow) + } + } + } +} diff --git a/src/plugins/follow_config/FollowConfig.qrc b/src/plugins/follow_config/FollowConfig.qrc new file mode 100644 index 000000000..61099f8f1 --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.qrc @@ -0,0 +1,5 @@ + + + FollowConfig.qml + +