diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md index 4d83fe9ca..79d837f50 100644 --- a/examples/standalone/scene_provider/README.md +++ b/examples/standalone/scene_provider/README.md @@ -54,7 +54,7 @@ gz topic -e -t /gui/camera/pose Echo camera tracking information: -``` +```bash gz topic -e -t /gui/currently_tracked ``` @@ -66,7 +66,7 @@ gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean Follow box from service (depricated): -``` +```bash gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: 2, follow_target: "box_model"' ``` diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index 640b1daee..7452450c0 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -20,10 +20,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -62,7 +62,7 @@ class CameraTrackingPrivate msgs::Boolean &_res); /// \brief Callback for a track message - /// \param[in] _msg Message consists of the target to track, type of tracking, offset and pgain. + /// \param[in] _msg Message is of type CamerTrack. public: void OnTrackSub(const msgs::CameraTrack &_msg); /// \brief Callback for a follow request @@ -214,7 +214,7 @@ void CameraTrackingPrivate::Initialize() // track this->trackTopic = "/gui/track"; - this->node.Subscribe(this->trackTopic, + this->node.Subscribe(this->trackTopic, &CameraTrackingPrivate::OnTrackSub, this); gzmsg << "Tracking topic on [" << this->trackTopic << "]" << std::endl; @@ -272,7 +272,7 @@ bool CameraTrackingPrivate::OnFollow(const msgs::StringMsg &_msg, void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg) { std::lock_guard lock(this->mutex); - gzmsg << "Got new track message."<< std::endl; + gzmsg << "Got new track message." << std::endl; if (_msg.track_mode() != gz::msgs::CameraTrack::USE_LAST) { @@ -280,11 +280,11 @@ void CameraTrackingPrivate::OnTrackSub(const msgs::CameraTrack &_msg) } if (!_msg.follow_target().empty()) { - this->selectedFollowTarget = _msg.follow_target(); + this->selectedFollowTarget = _msg.follow_target(); } if (!_msg.track_target().empty()) { - this->selectedTrackTarget = _msg.track_target(); + this->selectedTrackTarget = _msg.track_target(); } if (_msg.follow_target().empty() && _msg.track_target().empty() && _msg.track_mode() != gz::msgs::CameraTrack::USE_LAST) @@ -428,7 +428,8 @@ void CameraTrackingPrivate::OnRender() // reset track mode if target node got removed if (!this->selectedFollowTarget.empty()) { - rendering::NodePtr targetFollow = this->scene->NodeByName(this->selectedFollowTarget); + rendering::NodePtr targetFollow = this->scene->NodeByName( + this->selectedFollowTarget); if (!targetFollow && !this->selectedTargetWait) { this->camera->SetFollowTarget(nullptr); @@ -437,7 +438,8 @@ void CameraTrackingPrivate::OnRender() } if (!this->selectedTrackTarget.empty()) { - rendering::NodePtr targetTrack = this->scene->NodeByName(this->selectedTrackTarget); + rendering::NodePtr targetTrack = this->scene->NodeByName( + this->selectedTrackTarget); if (!targetTrack && !this->selectedTargetWait) { this->camera->SetTrackTarget(nullptr); @@ -449,18 +451,22 @@ void CameraTrackingPrivate::OnRender() return; rendering::NodePtr selectedFollowTargetTmp = this->camera->FollowTarget(); rendering::NodePtr selectedTrackTargetTmp = this->camera->TrackTarget(); - if (!this->selectedTrackTarget.empty() || !this->selectedFollowTarget.empty()) + if (!this->selectedTrackTarget.empty() || + !this->selectedFollowTarget.empty()) { - rendering::NodePtr targetFollow = this->scene->NodeByName(this->selectedFollowTarget); - rendering::NodePtr targetTrack = this->scene->NodeByName(this->selectedTrackTarget); + rendering::NodePtr targetFollow = this->scene->NodeByName( + this->selectedFollowTarget); + rendering::NodePtr targetTrack = this->scene->NodeByName( + this->selectedTrackTarget); if (targetFollow || targetTrack) { if (this->trackMode == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK || this->trackMode == gz::msgs::CameraTrack::FOLLOW || this->trackMode == gz::msgs::CameraTrack::FOLLOW_LOOK_AT ) { - if (!selectedFollowTargetTmp || targetFollow != selectedFollowTargetTmp - || this->newTrack) + if (!selectedFollowTargetTmp || + targetFollow != selectedFollowTargetTmp || + this->newTrack) { this->trackWorldFrame = false; this->camera->SetFollowTarget(targetFollow, @@ -489,8 +495,9 @@ void CameraTrackingPrivate::OnRender() } if (this->trackMode == gz::msgs::CameraTrack::TRACK) { - if (!selectedTrackTargetTmp || targetTrack != selectedTrackTargetTmp - || this->newTrack) + if (!selectedTrackTargetTmp || + targetTrack != selectedTrackTargetTmp || + this->newTrack) { this->trackWorldFrame = true; this->camera->SetFollowTarget(nullptr); @@ -547,11 +554,16 @@ CameraTracking::CameraTracking() if (this->dataPtr->trackMode == gz::msgs::CameraTrack::TRACK) { this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::TRACK); - this->dataPtr->trackMsg.set_track_target(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.set_track_target( + 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(); @@ -559,38 +571,58 @@ CameraTracking::CameraTracking() else if (this->dataPtr->trackMode == gz::msgs::CameraTrack::FOLLOW) { this->dataPtr->trackMsg.set_track_mode(gz::msgs::CameraTrack::FOLLOW); - this->dataPtr->trackMsg.set_follow_target(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_target( + 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) + 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.set_follow_target(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_track_mode( + gz::msgs::CameraTrack::FOLLOW_FREE_LOOK); + this->dataPtr->trackMsg.set_follow_target( + 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) + 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.set_follow_target(this->dataPtr->selectedFollowTarget); - this->dataPtr->trackMsg.set_track_target(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_track_mode( + gz::msgs::CameraTrack::FOLLOW_LOOK_AT); + this->dataPtr->trackMsg.set_follow_target( + this->dataPtr->selectedFollowTarget); + this->dataPtr->trackMsg.set_track_target( + 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); } @@ -641,7 +673,8 @@ void CameraTracking::LoadConfig(const tinyxml2::XMLElement *_pluginElem) } if (auto followPGainElem = _pluginElem->FirstChildElement("follow_pgain")) { - this->dataPtr->followPGain = std::stod(std::string(followPGainElem->GetText())); + 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; @@ -657,7 +690,8 @@ void CameraTrackingPrivate::HandleKeyRelease(events::KeyReleaseOnScene *_e) if (_e->Key().Key() == Qt::Key_Escape) { this->trackMode = gz::msgs::CameraTrack::NONE; - if (!this->selectedFollowTarget.empty() || !this->selectedTrackTarget.empty() ) + if (!this->selectedFollowTarget.empty() || + !this->selectedTrackTarget.empty()) { this->selectedFollowTarget = std::string(); this->selectedTrackTarget = std::string(); @@ -686,7 +720,6 @@ bool CameraTracking::eventFilter(QObject *_obj, QEvent *_event) // Standard event processing return QObject::eventFilter(_obj, _event); } - } // namespace gz::gui::plugins // Register this plugin diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc index f3117077e..6eefd4731 100644 --- a/src/plugins/camera_tracking_config/CameraTrackingConfig.cc +++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include #include @@ -21,6 +22,7 @@ #include #include +#include #include #include @@ -86,7 +88,8 @@ void CameraTrackingConfig::LoadConfig(const tinyxml2::XMLElement *) // Track target pose service this->dataPtr->cameraTrackingTopic = "/gui/track"; this->dataPtr->trackingPub = - this->dataPtr->node.Advertise(this->dataPtr->cameraTrackingTopic); + this->dataPtr->node.Advertise( + this->dataPtr->cameraTrackingTopic); gzmsg << "CameraTrackingConfig: Tracking topic publisher advertised on [" << this->dataPtr->cameraTrackingTopic << "]" << std::endl; @@ -112,7 +115,7 @@ bool CameraTrackingConfig::eventFilter(QObject *_obj, QEvent *_event) ///////////////////////////////////////////////// void CameraTrackingConfig::SetTracking( double _tx, double _ty, double _tz, double _tp, - double _fx,double _fy, double _fz, double _fp) + double _fx, double _fy, double _fz, double _fp) { if (!this->dataPtr->newTrackingUpdate) { diff --git a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh index 4ec79d6e2..5080d8ea6 100644 --- a/src/plugins/camera_tracking_config/CameraTrackingConfig.hh +++ b/src/plugins/camera_tracking_config/CameraTrackingConfig.hh @@ -18,6 +18,8 @@ #define GZ_GUI_PLUGINS_CAMERATRACKINGCONFIG_HH_ #include +#include +#include #include "gz/gui/Plugin.hh"