diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md index 3183a83e4..ce2da00fc 100644 --- a/examples/standalone/scene_provider/README.md +++ b/examples/standalone/scene_provider/README.md @@ -9,7 +9,7 @@ plugin to update the scene using Gazebo Transport. ## Build -``` +```bash cd examples/standalone/scene_provider mkdir build cd build @@ -21,14 +21,14 @@ make In one terminal, start the scene provider: -``` +```bash cd examples/standalone/scene_provider/build ./scene_provider ``` On another terminal, start the example config: -``` +```bash gz gui -c examples/config/scene3d.config ``` @@ -42,24 +42,48 @@ Some commands to test camera tracking with this demo: Move to box: -``` +```bash gz service -s /gui/move_to --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"' ``` Echo camera pose: -``` +```bash gz topic -e -t /gui/camera/pose ``` -Follow box: +Echo camera tracking information: +```bash +gz topic -e -t /gui/currently_tracked ``` -gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"' + +Follow box from track topic: + +```bash +gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: { name: "box_model"}' ``` -Update follow offset: +Follow box from track topic: +```bash +gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model", follow_offset: {x: -1, y: 0, z: 1}' ``` + +Update follow offset from track topic: + +```bash +gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: {name: "box_model"}, follow_offset: {x: -1, y: 0, z: 1}' +``` + +Follow box from service (deprecated): + +```bash +gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"' +``` + +Update follow offset from follow offset service (deprecated): + +```bash gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5' ``` diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 01f7fc687..81ca4d848 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -116,6 +116,7 @@ endfunction() # Plugins add_subdirectory(camera_fps) add_subdirectory(camera_tracking) +add_subdirectory(camera_tracking_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 172bb6e05..2042459d4 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) 2024 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. @@ -16,9 +17,13 @@ */ #include +#include #include +#include #include +#include +#include #include #include #include @@ -59,6 +64,10 @@ class CameraTrackingPrivate public: bool OnMoveTo(const msgs::StringMsg &_msg, msgs::Boolean &_res); + /// \brief Callback for a track message + /// \param[in] _msg Message is of type CameraTrack. + public: void OnTrackSub(const msgs::CameraTrack &_msg); + /// \brief Callback for a follow request /// \param[in] _msg Request message to set the target to follow. /// \param[in] _res Response data @@ -96,27 +105,42 @@ class CameraTrackingPrivate //// \brief Pointer to the rendering scene public: rendering::ScenePtr scene = nullptr; + /// \brief Target to track + public: std::string selectedTrackTarget; + /// \brief Target to follow - public: std::string followTarget; + public: std::string selectedFollowTarget; - /// \brief Wait for follow target - public: bool followTargetWait = false; + /// \brief Wait for target to track + public: bool selectedTargetWait = false; /// \brief Offset of camera from target being followed - public: math::Vector3d followOffset = math::Vector3d(-5, 0, 3); + public: math::Vector3d followOffset = math::Vector3d(-3, 0, 2); + + /// \brief Offset on target to be tracked + public: math::Vector3d trackOffset = math::Vector3d(0, 0, 0); - /// \brief Flag to indicate the follow offset needs to be updated - public: bool followOffsetDirty = false; + /// \brief Camera tracking message + public: gz::msgs::CameraTrack trackMsg; - /// \brief Flag to indicate the follow offset has been updated - public: bool newFollowOffset = true; + /// \brief Flag to indicate new tracking + public: bool newTrack = true; + + /// \brief Track P gain + public: double trackPGain = 0.01; /// \brief Follow P gain public: double followPGain = 0.01; - /// \brief True follow the target at an offset that is in world frame, - /// false to follow in target's local frame - public: bool followWorldFrame = false; + /// \brief Free Look P gain + public: double freeLookPGain = 1.0; + + /// \brief Default track mode to None + public: int trackMode = gz::msgs::CameraTrack::NONE; + + /// \brief True track the target at an offset that is in world frame, + /// false to track in target's local frame + public: bool trackWorldFrame = false; /// \brief Last move to animation time public: std::chrono::time_point prevMoveToTime; @@ -136,14 +160,20 @@ class CameraTrackingPrivate /// \brief Move to service public: std::string moveToService; + /// \brief Follow service (deprecated) + public: std::string followService; + + /// \brief Follow offset service (deprecated) + public: std::string followOffsetService; + /// \brief The pose set from the move to pose service. public: std::optional moveToPoseValue; - /// \brief Follow service - public: std::string followService; + /// \brief Track topic + public: std::string trackTopic; - /// \brief Follow offset service - public: std::string followOffsetService; + /// \brief Track status topic + public: std::string trackStatusTopic; /// \brief Camera pose topic public: std::string cameraPoseTopic; @@ -151,6 +181,9 @@ class CameraTrackingPrivate /// \brief Move to pose service public: std::string moveToPoseService; + /// \brief Camera pose publisher + public: transport::Node::Publisher trackStatusPub; + /// \brief Camera pose publisher public: transport::Node::Publisher cameraPosePub; @@ -192,7 +225,21 @@ void CameraTrackingPrivate::Initialize() this->node.Advertise(this->followService, &CameraTrackingPrivate::OnFollow, this); gzmsg << "Follow service on [" - << this->followService << "]" << std::endl; + << this->followService << "] (deprecated)" << std::endl; + + // track + this->trackTopic = "/gui/track"; + this->node.Subscribe(this->trackTopic, + &CameraTrackingPrivate::OnTrackSub, this); + gzmsg << "Tracking topic on [" + << this->trackTopic << "]" << std::endl; + + // tracking status + this->trackStatusTopic = "/gui/currently_tracked"; + this->trackStatusPub = + this->node.Advertise(this->trackStatusTopic); + gzmsg << "Tracking status topic on [" + << this->trackStatusTopic << "]" << std::endl; // move to pose service this->moveToPoseService = @@ -214,7 +261,7 @@ void CameraTrackingPrivate::Initialize() this->node.Advertise(this->followOffsetService, &CameraTrackingPrivate::OnFollowOffset, this); gzmsg << "Follow offset service on [" - << this->followOffsetService << "]" << std::endl; + << this->followOffsetService << "] (deprecated)" << std::endl; } ///////////////////////////////////////////////// @@ -233,12 +280,60 @@ bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg, msgs::Boolean &_res) { std::lock_guard lock(this->mutex); - this->followTarget = _msg.data(); + this->selectedFollowTarget = _msg.data(); _res.set_data(true); + + this->trackMode = gz::msgs::CameraTrack::FOLLOW; + + this->newTrack = true; return true; } +///////////////////////////////////////////////// +void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg) +{ + std::lock_guard lock(this->mutex); + gzmsg << "Got new track message." << std::endl; + + if (_msg.track_mode() != gz::msgs::CameraTrack::USE_LAST) + { + this->trackMode = _msg.track_mode(); + } + if (!_msg.follow_target().name().empty()) + { + this->selectedFollowTarget = _msg.follow_target().name(); + } + if (!_msg.track_target().name().empty()) + { + this->selectedTrackTarget = _msg.track_target().name(); + } + if (_msg.follow_target().name().empty() && _msg.track_target().name().empty() + && _msg.track_mode() != gz::msgs::CameraTrack::USE_LAST) + { + gzmsg << "Track and Follow target names empty."<< std::endl; + } + if (_msg.has_follow_offset()) + { + this->followOffset = msgs::Convert(_msg.follow_offset()); + } + if (_msg.has_track_offset()) + { + this->trackOffset = msgs::Convert(_msg.track_offset()); + } + if (_msg.track_pgain() > 0.00001) + { + this->trackPGain = _msg.track_pgain(); + } + if (_msg.follow_pgain() > 0.00001) + { + this->followPGain = _msg.follow_pgain(); + } + + this->newTrack = true; + return; +} + ///////////////////////////////////////////////// void CameraTrackingPrivate::OnMoveToComplete() { @@ -256,9 +351,9 @@ bool CameraTrackingPrivate::OnFollowOffset(const msgs::Vector3d &_msg, msgs::Boolean &_res) { std::lock_guard lock(this->mutex); - if (!this->followTarget.empty()) + if (!this->selectedFollowTarget.empty()) { - this->newFollowOffset = true; + this->newTrack = true; this->followOffset = msgs::Convert(_msg); } @@ -364,66 +459,114 @@ void CameraTrackingPrivate::OnRender() } } - // Follow + // Track { - GZ_PROFILE("CameraTrackingPrivate::OnRender Follow"); - // reset follow mode if target node got removed - if (!this->followTarget.empty()) + GZ_PROFILE("CameraTrackingPrivate::OnRender Track"); + // reset track mode if target node got removed + if (!this->selectedFollowTarget.empty()) { - rendering::NodePtr target = this->scene->NodeByName(this->followTarget); - if (!target && !this->followTargetWait) + rendering::NodePtr targetFollow = this->scene->NodeByName( + this->selectedFollowTarget); + if (!targetFollow && !this->selectedTargetWait) { this->camera->SetFollowTarget(nullptr); + this->selectedFollowTarget.clear(); + } + } + if (!this->selectedTrackTarget.empty()) + { + rendering::NodePtr targetTrack = this->scene->NodeByName( + this->selectedTrackTarget); + if (!targetTrack && !this->selectedTargetWait) + { this->camera->SetTrackTarget(nullptr); - this->followTarget.clear(); + this->selectedTrackTarget.clear(); } } if (!this->moveToTarget.empty()) return; - rendering::NodePtr followTargetTmp = this->camera->FollowTarget(); - if (!this->followTarget.empty()) + rendering::NodePtr selectedFollowTargetTmp = this->camera->FollowTarget(); + rendering::NodePtr selectedTrackTargetTmp = this->camera->TrackTarget(); + if (!this->selectedTrackTarget.empty() || + !this->selectedFollowTarget.empty()) { - rendering::NodePtr target = scene->NodeByName( - this->followTarget); - if (target) + rendering::NodePtr targetFollow = this->scene->NodeByName( + this->selectedFollowTarget); + rendering::NodePtr targetTrack = this->scene->NodeByName( + this->selectedTrackTarget); + if (targetFollow || targetTrack) { - if (!followTargetTmp || target != followTargetTmp - || this->newFollowOffset) + if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK || + this->trackMode == gz::msgs::CameraTrack::FOLLOW || + this->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT ) { - this->camera->SetFollowTarget(target, - this->followOffset, - this->followWorldFrame); - this->camera->SetFollowPGain(this->followPGain); - - this->camera->SetTrackTarget(target); - // found target, no need to wait anymore - this->newFollowOffset = false; - this->followTargetWait = false; + if (!selectedFollowTargetTmp || + targetFollow != selectedFollowTargetTmp || + this->newTrack) + { + this->trackWorldFrame = false; + this->camera->SetFollowTarget(targetFollow, + this->followOffset, + this->trackWorldFrame); + if (this->trackMode == gz::msgs::CameraTrack::FOLLOW) + { + this->camera->SetTrackTarget(targetFollow); + this->camera->SetTrackPGain(this->followPGain); + this->camera->SetFollowPGain(this->trackPGain); + } + if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT) + { + this->camera->SetTrackTarget(targetTrack); + this->camera->SetTrackPGain(this->followPGain); + this->camera->SetFollowPGain(this->trackPGain); + } + if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK) + { + this->camera->SetTrackTarget(nullptr); + this->camera->SetFollowPGain(this->freeLookPGain); + } + this->newTrack = false; + this->selectedTargetWait = false; + } } - else if (this->followOffsetDirty) + if (this->trackMode == gz::msgs::CameraTrack::TRACK) { - math::Vector3d offset = - this->camera->WorldPosition() - target->WorldPosition(); - if (!this->followWorldFrame) + if (!selectedTrackTargetTmp || + targetTrack != selectedTrackTargetTmp || + this->newTrack) { - offset = target->WorldRotation().RotateVectorReverse(offset); + this->trackWorldFrame = true; + this->camera->SetFollowTarget(nullptr); + this->camera->SetTrackTarget(targetTrack, + this->trackOffset, + this->trackWorldFrame); + this->camera->SetTrackPGain(this->trackPGain); + this->newTrack = false; + this->selectedTargetWait = false; } - this->camera->SetFollowOffset(offset); - this->followOffsetDirty = false; } } - else if (!this->followTargetWait) + else if (!this->selectedTargetWait) { + gzerr << "Unable to track target. Target: '" + << this->selectedTrackTarget << "' not found" << std::endl; gzerr << "Unable to follow target. Target: '" - << this->followTarget << "' not found" << std::endl; - this->followTarget.clear(); + << this->selectedFollowTarget << "' not found" << std::endl; + this->selectedFollowTarget.clear(); + this->selectedTrackTarget.clear(); } } - else if (followTargetTmp) + else { - this->camera->SetFollowTarget(nullptr); - this->camera->SetTrackTarget(nullptr); + if (selectedFollowTargetTmp) + { + this->camera->SetFollowTarget(nullptr); + } + if (selectedTrackTargetTmp) + { + this->camera->SetTrackTarget(nullptr); + } } } } @@ -443,6 +586,96 @@ CameraTracking::CameraTracking() auto poseMsg = msgs::Convert(this->dataPtr->camera->WorldPose()); this->dataPtr->cameraPosePub.Publish(poseMsg); } + if (this->dataPtr->trackStatusPub.HasConnections()) + { + if (this->dataPtr->trackMode == gz::msgs::CameraTrack::TRACK) + { + this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::TRACK); + this->dataPtr->trackMsg.mutable_track_target()->set_name( + this->dataPtr->selectedTrackTarget); + this->dataPtr->trackMsg.mutable_track_offset()->set_x( + this->dataPtr->trackOffset.X()); + this->dataPtr->trackMsg.mutable_track_offset()->set_y( + this->dataPtr->trackOffset.Y()); + this->dataPtr->trackMsg.mutable_track_offset()->set_z( + this->dataPtr->trackOffset.Z()); + this->dataPtr->trackMsg.set_track_pgain( + this->dataPtr->trackPGain); + this->dataPtr->trackMsg.clear_follow_target(); + this->dataPtr->trackMsg.clear_follow_offset(); + this->dataPtr->trackMsg.clear_follow_pgain(); + } + else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW) + { + this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW); + this->dataPtr->trackMsg.mutable_follow_target()->set_name( + this->dataPtr->selectedFollowTarget); + this->dataPtr->trackMsg.mutable_follow_offset()->set_x( + this->dataPtr->followOffset.X()); + this->dataPtr->trackMsg.mutable_follow_offset()->set_y( + this->dataPtr->followOffset.Y()); + this->dataPtr->trackMsg.mutable_follow_offset()->set_z( + this->dataPtr->followOffset.Z()); + this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain); + this->dataPtr->trackMsg.clear_track_target(); + this->dataPtr->trackMsg.clear_track_offset(); + this->dataPtr->trackMsg.clear_track_pgain(); + } + else if (this->dataPtr->trackMode == + gz::msgs::CameraTrack::FOLLOW_FREE_LOOK) + { + this->dataPtr->trackMsg.set_track_mode( + gz::msgs::CameraTrack::FOLLOW_FREE_LOOK); + this->dataPtr->trackMsg.mutable_follow_target()->set_name( + this->dataPtr->selectedFollowTarget); + this->dataPtr->trackMsg.mutable_follow_offset()->set_x( + this->dataPtr->followOffset.X()); + this->dataPtr->trackMsg.mutable_follow_offset()->set_y( + this->dataPtr->followOffset.Y()); + this->dataPtr->trackMsg.mutable_follow_offset()->set_z( + this->dataPtr->followOffset.Z()); + this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain); + this->dataPtr->trackMsg.clear_track_target(); + this->dataPtr->trackMsg.clear_track_offset(); + this->dataPtr->trackMsg.clear_track_pgain(); + } + else if (this->dataPtr->trackMode == + gz::msgs::CameraTrack::FOLLOW_LOOK_AT) + { + this->dataPtr->trackMsg.set_track_mode( + gz::msgs::CameraTrack::FOLLOW_LOOK_AT); + this->dataPtr->trackMsg.mutable_follow_target()->set_name( + this->dataPtr->selectedFollowTarget); + this->dataPtr->trackMsg.mutable_track_target()->set_name( + this->dataPtr->selectedTrackTarget); + this->dataPtr->trackMsg.mutable_follow_offset()->set_x( + this->dataPtr->followOffset.X()); + this->dataPtr->trackMsg.mutable_follow_offset()->set_y( + this->dataPtr->followOffset.Y()); + this->dataPtr->trackMsg.mutable_follow_offset()->set_z( + this->dataPtr->followOffset.Z()); + this->dataPtr->trackMsg.mutable_track_offset()->set_x( + this->dataPtr->trackOffset.X()); + this->dataPtr->trackMsg.mutable_track_offset()->set_y( + this->dataPtr->trackOffset.Y()); + this->dataPtr->trackMsg.mutable_track_offset()->set_z( + this->dataPtr->trackOffset.Z()); + this->dataPtr->trackMsg.set_follow_pgain(this->dataPtr->followPGain); + this->dataPtr->trackMsg.set_track_pgain(this->dataPtr->trackPGain); + } + else + { + this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::NONE); + this->dataPtr->trackMsg.clear_track_target(); + this->dataPtr->trackMsg.clear_track_offset(); + this->dataPtr->trackMsg.clear_track_pgain(); + this->dataPtr->trackMsg.clear_follow_target(); + this->dataPtr->trackMsg.clear_follow_offset(); + this->dataPtr->trackMsg.clear_follow_pgain(); + } + + this->dataPtr->trackStatusPub.Publish(this->dataPtr->trackMsg); + } }); this->dataPtr->timer->setInterval(1000.0 / 50.0); this->dataPtr->timer->start(); @@ -452,11 +685,39 @@ CameraTracking::CameraTracking() CameraTracking::~CameraTracking() = default; ///////////////////////////////////////////////// -void CameraTracking::LoadConfig(const tinyxml2::XMLElement *) +void CameraTracking::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { if (this->title.empty()) this->title = "Camera tracking"; + if (_pluginElem) + { + if (auto followTargetElem = _pluginElem->FirstChildElement("follow_target")) + { + this->dataPtr->selectedFollowTarget = followTargetElem->GetText(); + gzmsg << "CameraTracking: Loaded follow target from sdf [" + << this->dataPtr->selectedFollowTarget << "]" << std::endl; + this->dataPtr->selectedTargetWait = true; + } + if (auto followOffsetElem = _pluginElem->FirstChildElement("follow_offset")) + { + std::stringstream followOffsetStr; + followOffsetStr << std::string(followOffsetElem->GetText()); + followOffsetStr >> this->dataPtr->followOffset; + gzmsg << "CameraTracking: Loaded offset from sdf [" + << this->dataPtr->followOffset << "]" << std::endl; + this->dataPtr->newTrack = true; + } + if (auto followPGainElem = _pluginElem->FirstChildElement("follow_pgain")) + { + this->dataPtr->followPGain = std::stod( + std::string(followPGainElem->GetText())); + gzmsg << "CameraTracking: Loaded follow pgain from sdf [" + << this->dataPtr->followPGain << "]" << std::endl; + this->dataPtr->newTrack = true; + } + } + App()->findChild()->installEventFilter(this); } @@ -465,9 +726,12 @@ void CameraTrackingPrivate::HandleKeyRelease(events::KeyReleaseOnScene *_e) { if (_e->Key().Key() == Qt::Key_Escape) { - if (!this->followTarget.empty()) + this->trackMode = gz::msgs::CameraTrack::NONE; + if (!this->selectedFollowTarget.empty() || + !this->selectedTrackTarget.empty()) { - this->followTarget = std::string(); + this->selectedFollowTarget = std::string(); + this->selectedTrackTarget = std::string(); _e->accept(); } diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh index 68176e2c5..ae6833b9c 100644 --- a/src/plugins/camera_tracking/CameraTracking.hh +++ b/src/plugins/camera_tracking/CameraTracking.hh @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2024 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. @@ -30,12 +31,14 @@ namespace gz::gui::plugins /// and "follow". /// /// Services: + /// * `/gui/follow`: Set the user camera to follow a given target, + /// identified by name (deprecated). + /// * `/gui/follow/offset`: Set the offset for following (deprecated). /// * `/gui/move_to`: Move the user camera to look at a given target, /// identified by name. /// * `/gui/move_to/pose`: Move the user camera to a given pose. - /// * `/gui/follow`: Set the user camera to follow a given target, - /// identified by name. - /// * `/gui/follow/offset`: Set the offset for following. + /// * `/gui/track`: Set the user camera to follow a given target, + /// identified by name, offset, pgain, track type. /// /// Topics: /// * `/gui/camera/pose`: Publishes the current user camera pose. diff --git a/src/plugins/camera_tracking/CameraTracking.qml b/src/plugins/camera_tracking/CameraTracking.qml index 09c339f0b..a8c82d45e 100644 --- a/src/plugins/camera_tracking/CameraTracking.qml +++ b/src/plugins/camera_tracking/CameraTracking.qml @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2024 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. @@ -26,11 +27,12 @@ ColumnLayout { anchors.margins: 10 property string message: 'Services provided:
    ' + + '
  • /gui/follow/pose
    • ' + '
  • /gui/move_to
    • ' + '
  • /gui/move_to/pose
    • ' + - '
  • /gui/follow
    • ' + - '
  • /gui/follow/offset

Topics provided:
    ' + - '
  • /gui/camera/pose
' + '
  • /gui/track

    • Topics provided:
      ' + + '
    • /gui/camera/pose
      • '+ + '
    • /gui/currently_tracked
    ' Label { Layout.fillWidth: true diff --git a/src/plugins/camera_tracking_config/CMakeLists.txt b/src/plugins/camera_tracking_config/CMakeLists.txt new file mode 100644 index 000000000..b6877357c --- /dev/null +++ b/src/plugins/camera_tracking_config/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_gui_add_plugin(CameraTrackingConfig + SOURCES + CameraTrackingConfig.cc + QT_HEADERS + CameraTrackingConfig.hh +) diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc new file mode 100644 index 000000000..c61f8ac44 --- /dev/null +++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2024 CogniPilot Foundation + * Copyright (C) 2024 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 +#include +#include + +#include "gz/gui/Application.hh" +#include "gz/gui/Conversions.hh" +#include "gz/gui/GuiEvents.hh" +#include "gz/gui/MainWindow.hh" + +#include + +#include "CameraTrackingConfig.hh" + +/// \brief Private data class for CameraTrackingConfig +class gz::gui::plugins::CameraTrackingConfigPrivate +{ + + /// \brief Topic for track message + public: std::string cameraTrackingTopic; + + /// \brief tracking offset + public: math::Vector3d trackOffset{math::Vector3d(0.0, 0.0, 0.0)}; + + /// \brief track P gain + public: double trackPGain{0.01}; + + /// \brief Offset of camera from target being followed + public: math::Vector3d followOffset{math::Vector3d(-3.0, 0.0, -2.0)}; + + /// \brief Follow P gain + public: double followPGain{0.01}; + + public: transport::Node node; + + /// \brief Process updated track + public: void UpdateTracking(); + + /// \brief flag for updating + public: bool newTrackingUpdate = false; + + /// \brief track publisher + public: transport::Node::Publisher trackingPub; +}; + +using namespace gz; +using namespace gui; +using namespace plugins; + +///////////////////////////////////////////////// +CameraTrackingConfig::CameraTrackingConfig() + : gz::gui::Plugin(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +CameraTrackingConfig::~CameraTrackingConfig() = default; + +///////////////////////////////////////////////// +void CameraTrackingConfig::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Camera Tracking Config"; + + // Track target pose service + this->dataPtr->cameraTrackingTopic = "/gui/track"; + this->dataPtr->trackingPub = + this->dataPtr->node.Advertise( + this->dataPtr->cameraTrackingTopic); + gzmsg << "CameraTrackingConfig: Tracking topic publisher advertised on [" + << this->dataPtr->cameraTrackingTopic << "]" << std::endl; + + gui::App()->findChild< + MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool CameraTrackingConfig::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == events::Render::kType) + { + if (this->dataPtr->newTrackingUpdate) + { + this->dataPtr->UpdateTracking(); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void CameraTrackingConfig::SetTracking( + double _tx, double _ty, double _tz, double _tp, + double _fx, double _fy, double _fz, double _fp) +{ + if (!this->dataPtr->newTrackingUpdate) + { + this->dataPtr->trackOffset = math::Vector3d( + _tx, _ty, _tz); + this->dataPtr->followOffset = math::Vector3d( + _fx, _fy, _fz); + this->dataPtr->trackPGain = _tp; + this->dataPtr->followPGain = _fp; + gzmsg << "CameraTrackingConfig: Track: Offset(" + << this->dataPtr->trackOffset << "), PGain(" + << this->dataPtr->trackPGain << ")" << std::endl; + gzmsg << "CameraTrackingConfig: Follow: Offset(" + << this->dataPtr->followOffset << "), PGain(" + << this->dataPtr->followPGain << ")" << std::endl; + this->dataPtr->newTrackingUpdate = true; + } +} + +///////////////////////////////////////////////// +void CameraTrackingConfigPrivate::UpdateTracking() +{ + // Track + msgs::CameraTrack trackingMsg; + trackingMsg.set_track_mode(msgs::CameraTrack::USE_LAST); + trackingMsg.mutable_track_offset()->set_x(this->trackOffset.X()); + trackingMsg.mutable_track_offset()->set_y(this->trackOffset.Y()); + trackingMsg.mutable_track_offset()->set_z(this->trackOffset.Z()); + trackingMsg.mutable_follow_offset()->set_x(this->followOffset.X()); + trackingMsg.mutable_follow_offset()->set_y(this->followOffset.Y()); + trackingMsg.mutable_follow_offset()->set_z(this->followOffset.Z()); + trackingMsg.set_follow_pgain(this->followPGain); + trackingMsg.set_track_pgain(this->trackPGain); + + this->trackingPub.Publish(trackingMsg); + gzmsg << "CameraTrackingConfig: Publishing message." << std::endl; + this->newTrackingUpdate = false; +} + +// Register this plugin +GZ_ADD_PLUGIN(CameraTrackingConfig, + gui::Plugin) diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh new file mode 100644 index 000000000..dca58a52e --- /dev/null +++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2024 CogniPilot Foundation + * Copyright (C) 2024 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_CAMERATRACKINGCONFIG_HH_ +#define GZ_GUI_PLUGINS_CAMERATRACKINGCONFIG_HH_ + +#include +#include +#include + +#include "gz/gui/Plugin.hh" + +namespace gz +{ +namespace gui +{ +namespace plugins +{ + class CameraTrackingConfigPrivate; + + class CameraTrackingConfig : public Plugin + { + Q_OBJECT + + /// \brief Constructor + public: CameraTrackingConfig(); + + /// \brief Destructor + public: virtual ~CameraTrackingConfig(); + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *) + override; + + /// \brief Set the tracking camera, requested from the GUI. + /// \param[in] _tx The track offset in x + /// \param[in] _ty The track offset in y + /// \param[in] _tz The track offset in z + /// \param[in] _tp The track camera P gain + /// \param[in] _fx The follow offset in x + /// \param[in] _fy The follow offset in y + /// \param[in] _fz The follow offset in z + /// \param[in] _fp The follow camera P gain + public slots: void SetTracking( + double _tx, double _ty, double _tz, double _tp, + double _fx, double _fy, double _fz, double _fp); + + // 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/camera_tracking_config/CameraTrackingConfig.qml b/src/plugins/camera_tracking_config/CameraTrackingConfig.qml new file mode 100644 index 000000000..91c5e144a --- /dev/null +++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.qml @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2024 CogniPilot Foundation + * Copyright (C) 2024 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 track camera pose + property double xTrackPose: 0.0 + // Y track camera pose + property double yTrackPose: 0.0 + // Z track camera pose + property double zTrackPose: 0.0 + // P Gain track camera pose + property double pGainTrack: 0.01 + // X camera follow distance + property double xFollowOffset: -3.0 + // Y camera follow distance + property double yFollowOffset: 0.0 + // Z camera follow distance + property double zFollowOffset: 2.0 + // P Gain camera follow distance + property double pGainFollow: 0.01 + + GridLayout { + Layout.fillWidth: true + Layout.margins: 2 + columns: 2 + + // X Track Offset + Label { + id: xTrackPoseLabel + text: "Track Offset X (m)" + color: "dimgrey" + } + GzSpinBox { + id: xTrackPoseField + Layout.fillWidth: true + value: xTrackPose + maximumValue: 1000.0 + minimumValue: -1000.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + xTrackPose = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow) + } + } + // Y Track Offset + Label { + id: yTrackPoseLabel + text: "Track Offset Y (m)" + color: "dimgrey" + } + GzSpinBox { + id: yTrackPoseField + Layout.fillWidth: true + value: yTrackPose + maximumValue: 1000.0 + minimumValue: -1000.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + yTrackPose = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow) + } + } + // Z Track Offset + Label { + id: zTrackPoseLabel + text: "Track Offset Z (m)" + color: "dimgrey" + } + GzSpinBox { + id: zTrackPoseField + Layout.fillWidth: true + value: zTrackPose + maximumValue: 1000.0 + minimumValue: -1000.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + zTrackPose = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow) + } + } + // P Gain track + Label { + id: pGainTrackLabel + text: "Track P Gain" + color: "dimgrey" + } + GzSpinBox { + id: pGainTrackField + Layout.fillWidth: true + value: pGainTrack + maximumValue: 1.0 + minimumValue: 0.001 + decimals: 3 + stepSize: 0.01 + onEditingFinished:{ + pGainTrack = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset, pGainFollow) + } + } + // X Follow Offset + Label { + id: xFollowOffsetLabel + text: "Follow Offset X (m)" + color: "dimgrey" + } + GzSpinBox { + id: xFollowOffsetField + Layout.fillWidth: true + value: xFollowOffset + maximumValue: 100.0 + minimumValue: -100.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + xFollowOffset = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow) + } + } + // Y Follow Offset + Label { + id: yFollowOffsetLabel + text: "Follow Offset Y (m)" + color: "dimgrey" + } + GzSpinBox { + id: yFollowOffsetField + Layout.fillWidth: true + value: yFollowOffset + maximumValue: 100.0 + minimumValue: -100.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + yFollowOffset = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow) + } + } + // Z Follow Offset + Label { + id: zFollowOffsetLabel + text: "Follow Offset Z (m)" + color: "dimgrey" + } + GzSpinBox { + id: zFollowOffsetField + Layout.fillWidth: true + value: zFollowOffset + maximumValue: 100.0 + minimumValue: -100.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + zFollowOffset = value + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow) + } + } + // P Gain follow + Label { + id: pGainFollowLabel + text: "Follow Offset 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 + CameraTrackingConfig.SetTracking(xTrackPose, yTrackPose, zTrackPose, pGainTrack, xFollowOffset, yFollowOffset, zFollowOffset,pGainFollow) + } + } + } +} diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.qrc b/src/plugins/camera_tracking_config/CameraTrackingConfig.qrc new file mode 100644 index 000000000..5b9e0d08d --- /dev/null +++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.qrc @@ -0,0 +1,5 @@ + + + CameraTrackingConfig.qml + +